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Abstract 


Sophisticated robots can gready enhance the role of humans in space by relieving astronauts of 
low level, tedious assembly and maintenance chores and allowing them to concentrate on higher level 
tasks. Robots and astronauts can work together efficiendy, as a team; but the robot must be capable 
of accomplishing complex operations and yet be easy to use. Multiple cooperating manipulators are 
essential to dexterity and can broaden gready the types of activities the robot can achieve; adding 
adaptive control can ease gready robot usage by allowing the robot to change its own controller actions, 
without human intervention, in response to changes in its environment. Previous work in the Aerospace 
Robotics Laboratory (ARL) have shown the usefulness of a space robot with cooperating manipulators. 
The research presented in this dissertation extends that work by adding adaptive control. 

To help achieve this high level of robot sophistication, this research made several advances to 
the field of nonlinear adaptive control of robotic systems. A nonlinear adaptive control algorithm 
developed originally for control of robots, but requiring joint positions as inputs, was extended here 
to handle the much more general case of manipulator endpoint-position commands. A new system 
modelling technique, called system concatenation was developed to simplify the generation of a system 
model for complicated systems, such as a free-flying multiple-manipulator robot system. Finally, the 
ft&£-space concept was introduced wherein the operators inputs specify only the robots task. The 
robots subsequent autonomous performance of each task still involves, of course, endpoint positions 
and joint configurations as subsets. 

The combination of these developments resulted in a new adaptive control framework that is capable 
of continuously providing full adaptation capability to the complex space-robot system in all modes 
of operation. The new adaptive control algorithm easily handles free-flying systems with multiple, 
interacting manipulators, and extends naturally to even larger systems. 

The new adaptive controller was experimentally demonstrated on an ideal testbed in the ARL — a 
first-ever experimental model of a multi-manipulator, free-flying space robot that is capable of capturing 
and manipulating free-floating objects without requiring human assistance. A graphical user interface 
enhanced the robot usability: it enabled an operator situated at a remote location to issue high-level 
task description commands to the robot, and to monitor robot activities as it then carried out each 
assignment autonomously. 
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Chapter 1 


Introduction 


This dissertation describes theoretical and experimental research on the nonlinear adaptive control of a 
free-flying space robot with cooperating manipulators. The research was conducted in the Aerospace 
Robotics Laboratory (ARL) at Stanford University from 1986 to 1992. 

1.1 Motivation 

Space presents new and exciting challenges. One challenge involves the construction and maintenance 
of large space structures. While the space program can rely solely on astronauts to perform these duties, 
doing so makes very inefficient use of their abilities. Robots will add great capabilities to the space 
program, but only if they possess sufficient dexterity and skill. Multiple cooperative manipulators are 
essential to dexterity; adaptive control helps significantly to provide the skill. 

1.1.1 What is Adaptive Control? 

Adaptive control can change its controller actions to assure that the system continues to perform at 
its best despite changes in the environment or to unknown payload and robot parameters. That is, it 
adapts. 

Adaptive control for robots is useful in several important, common situations: 1) When there is 
poor or no knowledge of the payload parameters, 2) When there are incomplete models of the robot, 
3) When there are changes in the environment. While a robust, nonadaptive, controller may provide 
the same protections as an adaptive controller in these situations, it typically does so with substantially 
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reduced performance. The added complexity of an adaptive controller wins back that lost performance. 

The most obvious situation in which to use adaptive control is for handling payloads that have 
unknown or poorly known physical properties — for example, when handling damaged satellites where 
the nature and extent of the damage are unknown. More generally, this capability relieves astronauts of 
the duty to inform the robot of the detailed physical properties of each payload the robot is to handle. 
While a comprehensive parts database can relieve much of this responsibility, adaptive control provides 
protection in cases when the database is not completely accurate or is lacking. 

Adaptive control also eases the basic controller design process. There are typically many aspects 
of the robot itself that are either poorly modelled or not modelled. It is difficult to develop accurate 
models for robots. In many cases, it is impossible to perform system identification to verify models of 
space systems on the ground 1 By providing appropriate adjustable parameters to the controller, adaptive 
control can adapt to model uncertainties to render their effects unimportant. 

Another important benefit of adaptive control in space robots is its ability to adapt to the gradual 
changes that are inevitable in all mechanical systems. Even if friction characteristics are well-characterized 
at the outset, they will not remain so as the robot ages. A deterministic change — such as inertial property 
variations as a robot uses up fuel — also can benefit from adaptive control. The controller will adaptively 
update the controller to track the changes without user intervention or preplanned gain scheduling. 
By adapting to system changes with time, servicing and reprogramming of the robots can be reduced 
significandy. 

LI. 2 What is Task-Space Control? 

As a space robot carries out a typical operation, it needs to employ many control modes. For instance, 
in acquiring a part for assembly, a free-flying robot would start typically in a joint-control mode, where 
each joint is controlled to a known, “home” location. It then would enter base-control mode as the 
robot thrusts to approach the pan. When the pan comes into view of the local cameras or sensors, the 
robot would utilize manipulator-endpoint-control to track and grasp the pan with the arm end effectors. 
After grasping the pan, the robot would switch to cooperative -control mode 2 . After assembling 

*Thc Space Shuttle Remote Manipulator System (RMS), for example, cannot support its own weight on the ground. 

2 Cooperative ^erf-control should be distinguished from master/slave control, where command inputs must be given 
to one “master" manipulator, and the other manipulator or manipulators follow the “master" according to some heuristics, 
typically ignoring the dynamics of the payload. In object-control, the command inputs are specified directly in terms of the 
desired motions of the object. The manipulators then cooperatively effect the requested object motion, taking into account 
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and releasing the pan, the robot might reenter joint-control while returning home or proceeding with 
the next operation. 

Rather than distinguishing the different control modes and supplying different controllers for each 
control mode, this thesis unifies all the control modes into the task-space framework. The task-space 
concept is a generalization of control modes. Task-space encompasses all control objectives, such as 
object position, end-effector position, or joint positions. Choosing a particular task-space control vector 
is tantamount to choosing the control mode. A task-space control vector, however, does not have to 
be restricted to a single type of control mode. A task-space control vector for a multiple manipulator 
robot, for example, can represent endpoint control for one manipulator and joint control for the other. 
As another example, a free-flying space robot can choose a task-space control vector to represent the 
control of the positions of both the payload object and the free-flying base. 

This thesis develops a task-space controller that effects control for any choice of task-space control 
vector. Because the controller is formulated in task-space, it treats all control modes equally. Switching 
control modes essentially entails switching only the task-space control vector. The basic controller 
structure is unchanged, and a smooth transition is achieved. 

1.2 Research Goals 

The goal of this research effort is to develop an adaptive control framework that is sophisticated enough 
to control a free-flying space robot, yet broad enough to be generally applicable. The resulting adaptive 
control must meet the following requirements: 

• The adaptive control must furnish adaptation and control to multiple, cooperating manipulators 
from a free-flying base and also to simpler, single-manipulator fixed-base robots within the same 
framework. 

• The adaptive control must be able to supply object-level control during cooperative manipulation. 

• The adaptive control must provide total system adaptation. 

• The adaptive control must have task- space control capability to provide multiple control modes 
and to allow “graceful” transitions between control modes. 


the dynamics of the complete system. 
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• The adaptive control must be implementable . . 


Multiple manipulators vasdy increase the numbers and types of tasks a robot can perform. They 
provide dexterity well beyond what a single manipulator is capable of furnishing. Two manipulators 
grasping a long object at different locations provides better positioning accuracy than a single manipula- 
tor grasping one end or the middle of the object. Moreover, smaller motors in cooperating manipulators 
can provide the same torque capabilities as much larger motors of a single manipulator system. 

It is equally important that, in providing multiple-manipulator cooperative control, the adaptive 
algorithm be able to handle object-level control, wherein the operator directs the motion, or specifies 
the destination, of the manipulated object This eliminates the need for the physically and mentally 
demanding tasks of hand-in-glove teleoperation. It permits the human operator to focus totally on the 
task to be done. 

Total system adaptation is an important goal to achieve. It is more than just “payload adaptation” or 
just “manipulator adaptation”. Most existing nonlinear adaptive controllers for robots are geared toward 
identifying either the robots physical parameters, or unknown payload parameters, but not both. To 
provide maximum flexibility, the new adaptive controller needs to have the capability of adapting to 
both types of parameter changes simultaneously, without resorting to separate adaptation controllers. 
The adaptive algorithm also should be able to distinguish robot parameters from payload parameters, 
to allow for more intelligent setting of adaptation gains. Since robot parameters typically are better 
known and change more slowly, heavier weighting placed on adapting to changes in unknown payload 
parameters permits faster and more accurate adaptation, while still allowing adaptation to changes in 
the robot. 

Adaptive control should provide task-s pace control capability. Doing so makes adaptive control 
available at all times throughout a complex operation and ensures that the transitions between control 
modes are smooth. 

Because the adaptive algorithm must be implementable, the algorithm must not overwhelm the 
computational capabilities of present computers. The algorithm must be simple, but effective; and it 
must extend easily to even larger systems. 
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1.3 Contributions 

In meeting the challenging set of research goals, this research has generated an adaptive control frame- 
work that is very general and easily extensible to even larger, more complex systems than the free-flying 
robot with cooperating manipulators for which it was developed. Yet, this adaptive control framework 
is equally applicable to simple, single-arm fixed-base robots. 

This research takes advantage of the nonlinear, joint-space adaptive controllers already developed 
for single-arm, fixed-base robots [6] . Because a useful robot is expected to be capable of many control 
modes — including object-based control, endpoint control and joint control this research defines task- 
space control — encompassing all control modes — and extends the class of adaptive algorithms from 
joint-control to task-s pace control, capable of controlling in any mode utilizing the same adaptive 
control framework. To ease implementation of the new tasksp&ce adaptive controller for multiple 
cooperative manipulators, a new modelling technique called system concatenation is also developed. The 
combination of fcif£-space adaptive control and system concatenation results in a generalized adaptive 
control framework for rigid-link robotic system. 

The research contributions described in this dissertation include: 

• Framework . Development of a new general adaptive control framework — the adaptive task- space 
framework — that is capable of providing full adaptation to a free-flying space robot with two 
cooperating manipulators in all modes of operation. The generality allows the adaptive algorithm 
to extend readily beyond the scope of a single space robot to handle larger systems, including, for 
example, multiple robots with any number of manipulators. 

• Task Space. Extension and generalization of a joint-space, nonlinear, adaptive control algorithm, 
based on inverse dynamics, to control in the task space, which represents a broader class of control 
inputs, including, but not limited to, cooperative object control, as well as endpoint control and 
joint control. This extension provides a means for the adaptive controller to operate in all robot 
control modes. 

• System Concatenation Method Formulation of the system concatenation approach for efficient, 
incremental generation of system models for multiple, interacting systems. System concatenation 
takes full advantage of models already developed for each manipulator or robot subsystem to 
minimize the additional effort in deriving the total system models used for adaptation. This 
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formulation eliminates the need to develop closed-kinematic-chain equations of motion for 
multiple manipulators grasping a common object; and it decreases model complexity and increases 
computational speed without loss in fidelity. 

• Graphical- User-Interface Integration . Full integration of the new adaptive control algorithm into 
a hierarchical control architecture that includes a graphical user interface and a finite-state-table 
programming environment. The graphical user interface provides the user with an easy method 
for direcdy specifying object motions without engaging in fine manipulation details, while the 
finite-state-table programming provides a degree of autonomy by handling real-time interactions 
and necessary control-mode transitions to achieve a requested task. Modular design for the 
adaptive controller allows it to integrate with this hierarchical control architecture without losing 
its flexibility in handling multiple control modes. 

• Experimental Verification. Experimental verification of the new adaptive controller, in the hi- 
erarchical control environment, on the Multi-Manipulator Free-Flying Space Robot of the the 
Aerospace Robotics Laboratory (ARL), is shown in Figure 1.1. The robot utilizes an air bearing 
to reproduce the zero-g, drag-free conditions of space very accurately in two dimensions. This 
two-armed space robot is totally self-contained, carrying on-board fuel, power, computers, and 
wireless communications. It is capable of executing semi-autonomous tasks at the direction of a 
user situated at an off-board computer workstation. 

• Vision System. The authors development, for the ARL, of the “Point-Grabber II” vision system 
that, together with software drivers developed in ARL, is capable of tracking bright spots at 60 
Hz with better than 1/20 pixel resolution. Duplicates of this high-speed vision system serve as a 
surrogate global positioning sensor, and as a local on-board end-point sensor 3 . 

1 .4 Review of Related Research 

The new task-spzce adaptive control for a space robot presented in this thesis successfully integrates 

three control disciplines: cooperative manipulation, space robot control, and nonlinear adaptive control. 

Accordingly, this review of the related research is divided into three, albeit intersecting, sections. 


} This system has been adopted generically for experiments throughout the laboratory. 





Figure 1.1: Multi-Manipulator Free-Flying Space Robot 

This experimental space robot is used to verify the performance of the new adaptive controller 
developed in this thesis. The robot uses an air cushion to faithfully simulate the drag-free 
conditions of space in a two-dimensional plane. 
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1.4.1 Object-Based Cooperative Manipulation 

The most promising cooperative multiple manipulator control strategies involve object motion control. 
They are promising because of successful experimental demonstrations, and because of the easy interface 
they provide to higher level controllers; specifically, the higher level controller needs to specify only the 
object behavior, without having to worry about detailed manipulator motions. 

There are roughly three categories of object motion control: object position control, hybrid object 
force/position control, and object impedance control. With object position control, only the object’s 
position is controlled[25, 21], providing a mechanically “stiff” system. This can be problematic 
when coming into contact with the environment 4 . Hybrid object force/position control controls the 
object’s position in certain directions, while controlling the objects contact forces in other directions 
[41, 40, 12, 51, 16]. Hybrid schemes, however, are difficult to implement, requiring control “mode” 
changes as the object comes into contact, and the continual updating of the force and position degrees 
of freedom. On the other hand, object impedance control, developed by Schneider at ARL, provides a 
“compliant” response at all times by maintaining a relationship between the object’s position, velocity, 
and force on the environment [33, 32, 46]. No control switching is necessary as the object comes into 
contact with its environment. 

Schneider [32] provides a detailed review of nonadaptive multiple-manipulator control approaches. 

1.4.2 Nonadaptive Space Robotics Research 

Alexander [1] pioneered at ARL the research in high-fidelity space robotics experimentation with a 
demonstration of the control of a single-arm free-floating space robot. He did not utilize thrusters. 
The controller is essentially a computed-torque controller, with special partitioning of the inertia and 
nonlinear matrices. This partitioning, unfortunately, is difficult to extend to multiple-arm control. 
It is even more difficult to extend to adaptive control, because the partitioning must be performed 
symbolically to determine the adaptable parameters. 

Umetani and Yoshida [44, 45] introduced the Generalized Jacobian for maintaining zero momentum 
of a free-flying base while controlling its single manipulator. Their development did not consider 
multiple manipulators, and did not extend the Generalized Jacobian beyond zero momentum control. 

4 Rather than a smooth contact, the “stiffness” may cause the object bounce off the contacting surface, possibly damaging 
the object. 
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Umetani and Yoshida demonstrated their controller experimentally. 

Carignan [8] also performed experiments on manipulation from a free-flying robot using a sliding- 
mode controller. This controller only partially compensated for the interactions between the manipu- 
lator and the robot body. Unfortunately, the performance was limited by the experimental hardware. 

Koningstein and Ullman [20, 1 9] developed at ARL the System Jacobian for controlling a free-flying 
robot with multiple manipulators. The System Jacobian is essentially a variation of the Umetani and 
Yoshidas Generalized Jacobian that allows the control of the free-flying robot base and the handling 
of the constraints in a closed kinematic chain during cooperative manipulation. The System Jacobian 
also allows the control of the system momentum to be something other than zero. The controller is 
computed-torque, but implemented via Koningsteins efficient recursive algorithm. 

Ullman [43] further extended the work to a full hierarchical controller, incorporating graphical 
user interface (GUI), state-transition modules. Ullmaris research culminated in enabling the space 
robot to track, chase, and capture free-flying objects and deliver them to operator-specified locations, 
all with very simple operator commands. Dickson [10] made additional advances by developing a 
multiple- robot controller: Each space robot possesses a pair of arms; and they cooperatively manipulate 
a single object. 

A more detailed review of space robotics research can be found in Ullman [43]. 

None of the research described in this section incorporates adaptive control. The goal of this 
dissertation is to develop an adaptive control strategy that will retain all the capabilities of Ullmans 
controller for a free-flying robot with multiple, cooperating manipulators. 

1 .4.3 Nonlinear Adaptive Robot Control 

Craig [9] pioneered the research in nonlinear adaptive control for robotic manipulators. He developed 
a computed-torque adaptive controller that utilized the standard computed-torque controller as the 
standard controller block. A drawback of the adaptive portion of the controller is that it required 
measured joint accelerations and a time-consuming matrix inverse of the system inertia matrix. Craig 
successfully demonstrated the computed-torque adaptive algorithm on a PUMA arm. The computed- 
torque adaptive controller applies only to single manipulators under joint control. 

Slotine and Li [37, 39] derived the sliding-mode adaptive controller that does not require the 
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inverse of the inertia matrix. This controller is also an inverse-dynamics algorithm that attempts to 
cancel directly the nonlinear dynamics of the manipulator. The Lyapunov-based stability proof indicates 
that the tracking error approaches zero by “sliding” along a surface in a multi-dimensional phase space. 
Additional research extended the algorithm by adding exponentially forgetting least-squares [22] to 
improve convergence when there is not enough excitation. Slodne also extended the algorithm to 
handle endpoint, Cartesian-space control [38]. Slotine and Li have shown impressive capabilities of 
this adaptive controller experimentally, but, again, only for single fixed-base manipulators under joint 
control. 

Bayard and Wen [48, 6] developed theoretically an entire class of nonlinear adaptive control 
algorithms, again based on inverse dynamics. They have demonstrated that the new class of algorithms 
incorporates Craigs computed-torque adaptive algorithm. Bayard and Wens algorithms also, in general, 
do not require the inverse of the inertia matrix for adaptation. Although these algorithms are more 
general, they have been met with much less enthusiasm than Slotines sliding-mode adaptive control, 
mainly because of the complexity of the Lyapunov stability proofs. This added complexity does, 
however, allow them to prove the stability for a broad class of adaptive algorithms. Again, Bayard and 
Wen developed their algorithms for a single robotic manipulator under joint control. They did not 
perform any experiments. 

Hsia [13] and Onega and Spong [26] provide good reviews of the field of adaptive control for 
single, rigid robots. They compare and contrast the various approaches, and provide unified views of 
these adaptive algorithms. 

Zanutta [50] presented the only published experimental research on adaptive control of multiple 
cooperating manipulators, which he did at ARL. He utilized Schneider s object impedance control [32] 
in conjunction with an inverse-dynamics-based adaptive control that is similar to Slotines sliding- 
mode adaptive control. The object impedance control development, however, separates control into 
two parts: the calculation of necessary forces to effect desired object motion, and the calculation of 
required actuator torques to effect those forces at the grasp points of the manipulator. The adaptive 
control applies only to the first stage, adapting to unknown payloads. Adaptation to unknowns in the 
manipulators must be performed separately, by another means. Zanutta experimentally demonstrated 
the adaptive object impedance controller on a pair of fully-cooperating manipulators operating from a 
fixed base. 
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Additionally, Kim, Walker and Dionise [18] and Hu and Goldenbcrg [14] presented computer 
simulations of adaptive control of multiple manipulators. Kim et. al. relied on a master/slave 
configuration for the controller, and involved a complex set of equations and constraints to handle 
the closed kinematic loops formed by the two manipulators holding an object. Hu and Goldenbcrg 
considered a hybrid position- and force-control scheme, but required the inversion of the inertia matrix. 

The research presented in this thesis draws on many of the ideas from these researchers. It builds 
and extends Bayard and Wens class of adaptive algorithms to handle endpoint control — an extension 
that is then further developed to handle the more general task-s pace control. Koningsteins method 
of handling a closed kinematic chain in a control system is generalized, and culminates in the system 
concatenation concept. The Generalized Jacobian ideas of Umetani and Yoshida, and the Jacobian 
augmentation methods of Koningstein, Ullman, et. al., are further extended to the task space. 

A final note: In the system modelling arena, parallel theoretical research by Meldrum [23, 24] 
develops efficient, order-(jV) recursive algorithms for computing control and adaptive update equations. 
These algorithms are based on the spatial operator ideas with Kalman filtering techniques developed by 
Rodriquez [29, 30]. 

1.5 Readers Guide 

This thesis is organized into ten chapters and five appendices. Chapter 1 contains the motivation, 
research goals, contributions, a review of related research, and this Readers Guide. 

Chapter 2 develops the system modelling. It introduces the notation used throughout the disser- 
tation. Chapter 3 describes the control approach by reviewing a nonlinear adaptive control method 
developed by Bayard and Wen while they were at JPL. This joint-based adaptive controller serves as a 
starting point for the research presented in this thesis. The chapter discusses the basic elements of this 
joint-based adaptive algorithm, and highlights the salient aspects of the Lyapunov-based stability and 
convergence proof. 

Chapters 4, 5, and 6 describe developments that generalize the adaptive control framework: exten- 
sions of the joint-based adaptive controller, task ' space concept, and system concatenation. Although the 
latter two new developments can also stand on their own in a nonadaptive environment, their concepts 
enable the generalization of the adaptive control framework without greatly increasing the complexity 



12 


Chapter 1. Introduction 


of resulting algorithms. 

Chapter 7 merges the developments to formally present the task-space adaptive controller, and 
describes its properties. 

Chapter 8 presents the experimental system, and briefly describes the hierarchical control architec- 
ture. This chapter oudines the process of integrating the new adaptive controller into the experimental 
system. 

Chapter 9 presents experimental results using the new adaptive algorithm. The controller per- 
formance is compared with that of nonadaptive controllers operating either with accurate or with 
incomplete knowledge of system parameters. 

Chapter 10 brings this dissertation to an end with conclusions drawn from the research and 
suggestions for future research directions. 

Appendix A provides more detailed calculations used in the Lyapunov stability proofs. Appendix B 
includes the users manual for the Point Grabber II vision system, developed by the author. It describes 
the design and the software algorithms that enable it to achieve high-resolution sensing. Appendix C 
details the calibration procedures for calibrating the vision system and motor torque constants, and 
other sensors. Appendix D supplies the state transition diagrams used for each space robot task. Finally, 
Appendix E lists the controller input files, representing system parameters, sensor calibration values, 
controller gains and adaptive gains. 




Chapter 2 


Modelling 


This chapter introduces the notation used in this thesis, and it establishes the mathematical model, i.e., 
equations of motion, used to describe a robotic system. Section 2.1 presents the model most commonly 
utilized to describe rigid-link robots. Section 2.2 describes an alternate parameterization of the model 
that aids adaptive control. This parameterization, shown by An, et. al., [2] to be applicable to rigid 
robots, isolates the physical parameters of the robotic system into a single mathematical vector 1 . A 
simple example of a planar two-link manipulator serves to illustrate key concepts throughout the thesis. 


2.1 System Model 


The equations of motion of a manipulator system consisting of rigid bodies with n degrees of freedom 
(DOF) can be described by the following form: 

F = M(q)u + C(q, u)u + G(q) (2. 1 ) 

where q € R n is a mathematical vector of generalized coordinates 2 , u € R n is a mathematical vector 

of generalized speeds, F € R n is the mathematical vector of generalized active forces 3 , M(q) € R nXn 

J A mathematical vector is a collection of scalar terms placed into a vector notation. This should be distinguished from 
a physical vector, which describes a direction and magnitude in three-dimensional space. Mathematical vector and matrix 
notation can gready simplify the form of the equations presented in this thesis. 

2 Generalized coordinates are a convenient set of coordinates that is sufficient to describe the configuration of a system. 
Although these convenient coordinates are typically measurable quantities, such as angles and lengths, they do not have to be. 
In the modelling of systems with distributed flexibility, for example, generalized coordinates “measure” the mode shapes of the 
system, which are difficult to determine with a ruler and protractor. 

3 This is distinguished from a physical force vector. Each element of the generalized force represents the sum of the 
components of all physical forces effecting motion in a particular degree of freedom. 
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is the symmetric, positive-definite inertia matrix, C(q, u) G R nXn is the matrix of Coriolis and 
centrifugal terms, and G(q) G R n is the vector of gravity torques. The actual terms of the mass 
matrix, the Coriolis and centrifugal matrix, and the gravity torque vector depend on the choice of the 
generalized coordinates and generalized speeds. Additionally, for each choice of generalized speeds , 
the matrix C(q, u) is not uniquely defined, although the vector C(q, u)u is unique. An appropriate 
choice of C(q, u) can gready aid the development of an adaptive controller, as Chapter 3 will describe. 

The definition for generalized speeds is [15]: 

u = W(q)q + W,(q) (2.2) 

where W (q) is an n X n matrix of functions of q and time t > and W*(q) is an n X 1 vector of functions 
of q and time t. The W* (q) term is nonzero only if there are time-dependent and uncontrolled forcing 
functions, which are rare in roborics. 

In robotics, the common choice for the generalized coordinates, q, corresponds with the joint angles 
or positions of a manipulator, and u corresponds with the joint speeds 4 , q. The generalized forces, F, 
are assumed to be equal to the actuator torques or forces, r, arriving at the familiar form: 

r = M(q)q+C(q,q)q + G(q) (2.3) 

Stricdy speaking, however, r is itself a generalized quantity, and depends on the actuator arrangement 
for a manipulator. Each term of r represents only the net applied force or torque for each degree of 
freedom — forces and torques that may be supplied by a combination actuators. 

One benefit of the more generalized expression of Equation (2.1) is that appropriate choices for 
generalized speeds can ease implementation by simplifying the individual terms in the system matrices 5 , 
M(q), C(q, u), G(q). In robots with rcvolute joints, it is beneficial to choose generalized speeds to 
correspond with absolute rotational speeds of the links rather than relative speeds of each link with 
respect to its inboard link. In this special case, 

u = Wq (2.4) 

where W is a constant matrix of zeros and ones. For a thorough discussion of generalized coordinates 
and generalized speeds, see [15]. 

4 In this situation W (q) is the (constant) identity matrix, and Wt(q) is zero. 

5 Essentially, the kinematic configuration of a mechanical system may be developed utilizing a convenient set of coordinates, 
and the dynamic model may be developed utilizing a convenient set of generalized speeds, where the generalized speeds need 
not be restricted to be just the time derivatives of the chosen set of coordinates. 
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Example 



Figure 2.1: Planar Two-Link Fixed-Base Manipulator 

q i and qi represent the shoulder and elbow joint angles. m\, I\, and l\ are the mass, moment 
of inertia, and length of the upper link, while m 2 , 1 2 » and I 2 are those of the lower link. I* and 
Ij represent the center of mass locations of each link, which, for simplicity, are assumed to be 
along the central axis of each link. 


The simple planar two-link manipulator system illustrates the differences between these two forms 
for the equations of motion. The same example will be used throughout the rest of the thesis for 
clarifying new concepts. Figure 2.1 introduces the two-link arm system, the notation used for the 
physical parameters of the system, and the definitions of the generalized coordinates. The more 
standard equations of motion used by roboticists in for form of Equation (2.3) can be written as: 
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or 


r = M(q)q + C(q,q)q 


Because the arm operates in a plane perpendicular to the direction of gravity, there are no gravitational 
torques. 

Defining the generalized speeds as the absolute angular rotation of each link gives: 


U] 
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0 
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u 2 


<7i + <72 


1 
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92 


The generalized equations of motion, in the form of Equation (2.1), simplifies to: 
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F = M(q)u + C(q,u)u 


in which each term of F represents the total torque applied to each manipulator link, rather than the 
torque of each actuator: 
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( 2 . 8 ) 


2.2 Alternate Parameterization 


Researchers have shown that the dynamics of serial-kinematic-chained rigid robots allows an alternate 
parameterization of the equations of motion [2, 17, 4]. This linear-in-the-paramcters formulation 6 is 
well suited for adaptive control. It separates all the physical parameters of the system — the masses, 

6 More precisely, this is a parameterization that is linear in terms of a set of physical parameters of the system. The equations 
of motion are unchanged and remain nonlinear . The “parameterization” is simply another way of writing the same set of 
equations of motion. This property can be shown relatively easily by examining the Newton-Euler equations for the robotic 
system (see [2]). It is not clear whether this property is preserved in general for robotic systems that include closed kinematic 
chains. 
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moments of inertia, centers of mass locations, link lengths — from the states of the system, resulting in 
a single vector with p parameters, 0 € R p : 

F = M(q)u + C(q,u)u + G(q) = Y(q,u,u,u)0 (2.9) 

where Y(q, u, u, u) 6 R nXp is the regressor matrix 7 , containing functions of the generalized coordi- 
nates, generalized speeds and their time derivatives. An adaptive controller needs to adjust only this 
single parameter vector to adjust its model of the system. 


Example 

Utilizing the linear-in-the-parameters formulation, the equations of motion 8 for the simple planar arm 
example, using the more standard notation for robotics given by Equation (2.3), can be expressed as: 

2cos(q2)q\ + cos(g2)?2 j mi ^ 2 + m 2 l\ + I\ 

- sin (92)^1 (2<h + <72) J m 2 l i 2 + h 

0 q\ + 92 cos(q 2 )q\ + sm(q 2 )qi J [ 
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or alternatively using generalized speeds and total torques: 
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For both representations, 

r 

m\l \ 2 + m 2 l \ + I\ 

e = I m 2 l * 2 2 + I 2 

m 2 l\l 2 

is the parameter vector. The number of model parameters in 0, three, does not necessarily match the 

number of physical parameters, sue; and the model parameters can be complicated combinations of 

7 The two us in the notation for the regressor, Y(q, u, u, u), correspond to the us in the expression, C(q ? u)u. It is 
sometimes useful to distinguish between the two vectors to emphasize that Y(q, u, u, u) is nonlinear in u. Additionally, 
controllers that choose to use a control such as F = M(q)ud + C(q, u)ud + G(q) can be represented by F = Y(q, u, u<f, u<*)0. 
See [48] for a variety of different of controllers. 

8 Please note that the equations of motion are still nonlinear . 
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the physical parameters. Additionally, utilizing the model parameters, which has rank 3, one cannot 
uniquely determine all six physical parameters. This example illustrates that it is therefore often not 
possible to identify the individual link masses or moments of inertias or lengths from the model 
parameters. 


2.3 Summary 

This chapter provided two parameterizations for the equations of motion of robotic systems: the 
standard formulation used by most roboticists and the lincar4n'the-parameter$fotm\Az.t\on. The linear - 
in-thc-parameters formulation extracts the model parameters into a single mathematical vector and, thus, 
is well suited for adaptive control. Hence, the adaptive control algorithms presented in this dissertation 
takes full advantage of this alternate parameterization. 



Chapter 3 


Control Approach 


This chapter lays the groundwork for the development of the new &tf£-space adaptive controller 
presented later in the thesis. It also presents the key components of the stability and convergence proof 
that will be extended from joint-space control to task-space control. Section 3.1 introduces the set 
of control and adaptation laws, developed by Bayard and Wen, that serves as a basis for this research. 
The control laws are inverse-dynamics based, similar to the computed-torque control law; the adaptive 
parameter-update laws are derived from Lyapunov analysis to guarantee stability. Section 3.2 describes 
the properties of the controller, and Section 3.3 concludes with a stability and convergence proof, 
based on a Lyapunov function. The limitation to joint-space control of this baseline adaptive control 
algorithm will be extended in subsequent chapters. 

3.1 A Specific Adaptive Control Algorithm 

The Bayard and Wen adaptive control algorithms for robotic manipulators represent a class of inverse- 
dynamics-based controllers [5]. Bayard and Wen have shown, with appropriate choices of the control 
laws, that this class of algorithms incorporate Craigs computed-torque adaptive controller [9]. Most 
controllers in their class of algorithms, however, are easier to implement than the computed-torque 
adaptive controller, because they do not require measured accelerations or the inverse of the inertia 
matrix. In computer simulations, the Bayard and Wen algorithm also showed less sensitivity to 
sampling rate and velocity measurement noise than the Slotine and Li sliding-mode adaptive control 
[7], Additionally, Bayard and Wen have shown that their algorithms, in the nonadaptive case, are 
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exponentially stable [48]. 

This thesis concentrates on extending the Bayard and Wen algorithms to task-space control needed 
by the space robot. It should be noted, however, that similar extensions to the other adaptive algorithms 
will make them equally suitable for task-space control. 

Because most of the robotic adaptive control literature uses the more specific form of equations of 
motion defined by Equation (2.3), initial development of the adaptive control algorithm in this thesis 
will also use this form. Chapter 4 will generalize the results to utilize generalized speeds for equations 
of motion given by Equation (2.1). 

The development of the adaptive algorithm is illustrated with the following control law 1 . 

t = M(q)q<* + C(q,q d )q d + G(q) + K^q + K P q (3.1) 

where (•<*) are the desired trajectory quantities and 

0 = 0) - (0 (3.2) 

are the trajectory errors. K p > 0 is an n X n diagonal, position gain matrix, and > 0 is an n X n 
diagonal, velocity gain matrix. This is an inverse-dynamics controller, where the first three terms of 
Equation (3.1) represent feedforward 2 terms that cancel much of the dynamics of the plant, and the 
last two terms effectively provide proportional-derivative (PD) feedback to the system to account for 
residual trajectory-tracking errors. 

For adaptive control when the parameters are unknown, M(q), C(q, q<*), and G(q) are replaced 
by their estimates, M(q), C(q, q d ), and G(q). Using these estimated quantities and the linear-in-thc- 
parameters parameterization given in Equation (2.9), the adaptive control of Equation (3.1) is modified 
to: 

r = M(q)qd + C(q, q^q* + G(q) + K^q + K P q (3.3) 

This control law can also be written in terms of the parameter vector as either of the following: 

T ( = } Y(q,q d ,q (i5 q c ;)l + Kvq + Kpq (3.4) 

= Y (q, q dy q dj q d )0 + K^q + K/>q - Y (q, q dy q dy q d )0 (3.5) 

1 This is Control Law 5 in [6] or Control Law 9 in [47]. 

2 It can be argued that these are actually feedback terms, since they include measured states, q, but this author adopts a 
definition that a term is “feedback” only if it includes errors, i.e., the differences between commanded and actual quantities. 
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where the number, (2.9), beneath the equality sign is an equation reference, and 

o = e-e (3.6) 

is the parameter error vector. The last term in Equation (3.5) represents the difference between the 
nonadaptive and adaptive control laws. Note that all the unknowns in the system are contained in the 
parameter estimate vector, 9. 

The parameter adaptation law is: 

0 = -TY r (q, q<£, qd, q <0 (q + cq) (3-7) 

where T > 0 is an r X r diagonal, positive-definite matrix of parameter update gains, and c > 0 
is a scalar weighting between the velocity and position errors. This adaptive update law results from 
Lyapunov analysis, and is derived to guarantee the convergence of trajectory- tracking errors. 

3.2 Controller Properties 

The block diagram in Figure 3.1 illustrates the structure of the adaptive controller. Following are some 
properties of this controller: 

• Inverse-dynamics controller. The full nonlinear dynamics of the manipulator system is included 
in the “Inverse Dynamics” block. These calculations compensate for much of the nonlinear 
plant behavior, minimizing trajectory errors. This eases the burden on the PD “Feedback” block, 
allowing for more aggressive PD gains to handle any residual errors. The parameter vector, 6 , of 
the “Inverse Dynamics” block is adaptively updated to change the plant model. 

• Joint-space control ’ This standard controller applies for only joint control. The feedback is taken 
in joint-space. The controller thus operates to reduce errors in joint trajectories. The adaptation 
law also updates the parameters based on joint-space errors. 

• Tracking-error adaptive algorithm . The Bayard and Wen adaptive algorithms are tracking-error- 
based algorithms. The algorithms provide asymptotic convergence of the tracking error to zero, 
but do not guarantee parameter convergence. Parameter convergence requires sufficient excitation 
in the trajectories. This limitation poses no serious problems for most robotic activities where 
minimizing tracking error is the prime objective. 
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Figure 3.1: Block Diagram of Bayard and Wens Adaptive Controller 

T/iis is an inverse-dynamics joint-space controller. The plant model is used to feedforward 
the nonlinear dynamics to cancel much of the plant nonlinearity. The PD feedback and the 
adaptive updates are based on the joint trajectory errors. 


• Controller insensitivity to sensor noise. The inverse dynamics block of the controller uses the desired 
generalized speeds, rather than the measured values. This provides for lower sensitivity to minor 
sensor noise during regulation. Since there is no requested motion, the desired generalized speeds 
and accelerations are zero, which in turn causes all terms of the regressor, Y (q, q^, q<j> q<f), to be 
zero. This effectively disables the inverse dynamics feedforward block. During motion, feedback 
keeps the actual trajectories close to the desired ones, ensuring that the inverse dynamics block 
still produces adequate compensation for the nonlinear plant dynamics. 


• Adaptation insensitivity to sensor noise. Because the parameter adaptation law uses the same 
regressor, the adaptation is equally insensitive to minor sensor noise during regulation. There is 
no fear of parameter drift caused by measurement noise or small sensor biases. There is also no 
need for explicidy enabling and disabling parameter updates during operation. 
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3.3 Lyapunov Proof 

Bayard and Wen proved the stability and convergence of their adaptive algorithms based on a Lyapunov 
Function [6, 48, 47]. One typically chooses as candidate a Lyapunov Function — a scalar function — 
to represent the positive-definite “energy” contained in the tracking and parameter estimation errors. 
Proving that this energy is always decreasing demonstrates that the tracking and estimation errors 
converge to zero. This is equivalent to showing that the time-derivative of the chosen Lyapunov 
Function is always negative, which in turn establishes that the system is stable. 

Because the development of the &if£-space adaptive controller in subsequent chapters is based on 
the adaptive algorithm given in this chapter, the stability and convergence proof found in [47] will 
be repeated here, with some minor corrections and modifications. An outline of the proof describes, 
in words, the major steps involved in the proof. It also illustrates the typical process of developing 
Lyapunov proofs. The detailed steps of the stability proof follow in a separate section, with supporting 
calculations appearing in Appendix A. 

3.3. 1 Proof Outline 

The stability proof starts by examining the controller with exact plant knowledge and no adaptation. 
Consider the following Lyapunov Function for the system operating with the nonadaptive control law 
given by Equation (3.1): 

V (q, q, t) = ^q T M(q)q+ ^q T (Kp + cKK)q + cq r M(q)q (3.8) 

The first term is square in the joint-velocity tracking error, and as such represents the “energy” in the 
velocity errors 3 . Similarly, the second term is square in the joint-position tracking errors, and represents 
the “energy” in the position errors. Since the first two terms are squared terms, they are always positive. 
The third term is a cross product term between the velocity and position errors — this term is not 
guaranteed to be positive, so the positive-definiteness of Equation (3.8) must be shown. The scalar, 
c > 0, is a weighting on the cross term — c is the same weighting that appears in the adaptive parameter 
update law in Equation (3.7). 

Noting that M(q), K />, and Kv' are symmetric, the time-derivative of the Lyapunov Function can 

3 Recall that ^q r M(q)q is the kinetic energy of the manipulator system, and note the similarity with the first term of the 
Lyapunov Function of Equation (3.8). 



24 


Chapter 3. Control Approach 


be written as: 

v (q, q> 0 = q T M(q)q + ^q r M(q, q)q + q T (K P + cKy) q 

+cq X M(q)q + cq r M(q, q)q + cq T M(q)q (3.9) 

The stability proof involves showing that the chosen Lyapunov Function, Equation (3.8), is positive- 
definite; and that its derivative, Equation (3.9), is negative-semi-definite. It follows these steps: 

1. Show that the Lyapunov Function, V(q,q, is positive definite with the proper choice of 
c. Determine the appropriate bounds on c: any c that satisfies the bounds can be used in the 
parameter update law of Equation (3.7). 

2. Show that the Lyapunov Function is bounded if the velocity and position errors are bounded. 

3. Compute the time-derivative of V (q, q, t) in order to show with the following steps that it is 
never positive: 

(a) Use the definitions for trajectory errors, equations of motion, and the control law in 
Equations (3.2, 2.3, 3.4) to expand U(q, q, £). 

(b) Define a specific representation of C(q, q) that will eliminate and simplify terms in 

V(q, q ,t). 

(c) Determine the bounds on the remaining terms and show U(q, q, £) is negative-semi- 
definite. Doing so places requirements on the gain matrices, K p and Kk- 

This proves that if the initial errors arc bounded, V (q, q, £) remains uniformly bounded, which 
in turn implies that the trajectory errors remain uniformly bounded. 

4. Given the above, use Barbalat s theorem [27] and the boundedness of q to show that q and q 
tend to zero as t — ► oo, proving stability. 

The stability and convergence proof for the adaptive controller requires a modification of the 
Lyapunov Function to include a term that represents the “energy” in the parameter errors: 

Vi(q,q,0,<) = V(q,q ,t) + ^ T~ l 9 


(3.10) 
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Using the adaptive control law in Equation (3.5) and taking the time-derivative of the new Lyapunov 
function, V\ (q, q, 0, t), yields: 

VI (q, q,M) = V(q,q,t) + ^ T r _1 ^ + » T Y r (q,q < j,q <i ,q < j) (q + cq) (3.11) 

Noting that 6 = —0 and ^0 = 0, substitute the adaptive update law, Equation (3.7), to eliminate 
the last two terms of Equation (3.11). This reduces the adaptive case to the nonadaptive case of 
Equation (3.9). Making the same arguments shows that once again, q and q tend to zero as t — ► oo, 
proving stability for the adaptive algorithm. 


3.3.2 Proof Details 


This section presents the proof in some detail. The steps follow those derived in [47], but with the 
notation utilized in this thesis. They also correct some minor errors. It is useful to examine first the 
structure present in the equations of motion for a system of rigid bodies. 

The Lagrangian formulation of the equations of motion starts with the kinetic and potential energies 
for a system of rigid bodies: 

T = ±q T M(q)q 
U -- -q T r + g(q) 

where T is the kinetic energy, U is the total potential energy, of which g(q) is the gravitational potential 
energy component. Using the Lagrangian, L = T -U, and applying Lagrange’s Equation: 


( 3 . 12 ) 


d fdL\ dL 

dt \dq/ dq 

d (M(q)q) 1 9 (q T M(q)) , %( q) 

dt 2 dq q T dq 

= -r + M(q)q + M(q, q)q ) ^ + G ( q ) ^ 3 ‘ 13 ^ 

This results in the equations of motion in the form of Equation (2.3), which can be rewritten as: 


M(q)q = r - C(q, q)q - G(q) 


(3.14) 


where 

C(q,q)q * ^(q.4) - \ 
a gg(q) 

8q 


G(g) 


(3.16) 
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The stability proof uses the structure of the equations of motion demonstrated by Equations (3. 1 3-3.1 6) . 


Step 1: Positive-Definiteness of V. 

Define the following notation for bounds on matrices 4 : 


MA/ 

A 

inf <r min (M(q)) 

Mp 

A 

&min (Kp) 

M., 

A 

&min (Ky) 

7 M 

A 

sup a max (M(q)) 

q 

7p 

A 

^moi (Kp) 

7v 

A 

(Ky) 


where a m i n and a max denote the minimum and maximum singular values, respectively. Roughly, these 
singular values give the minimum and maximum “gain” of a matrix 5 . 

With these definitions, a lower bound on the Lyapunov Function of Equation (3.8) is^: 


q> 0 > j (^p + Wv) l|q|| 2 + ||q|| - ||q|| 

Using “perfect squares”, the cross term can be written as: 


(3.17) 


-2<hm ||q|| 


= ( e ||q|| — e 1 ||q||) 2 - cTA/f 2 ||q|| 2 - ctmc 2 ||q 

> -n M e 2 ||q|| 2 - C 7 M e~ 2 ||q|| 2 


( 3 . 18 ) 


4 The inf notation represents infimum, which is the lower bound of the specified set, but this bound does not have to be a 
member of the set. For example, inf 1 jx = 0, but 0 does not belong to the set { l/x}. Similarly, sup represents the supremum, 
or the upper bound of the specified set. 

5 The maximum and minimum singular values of a matrix, K, give the maximum and minimum “gain” of K, respectively, 
in a 2-norm sense. That is, 

l|Ku|l 


drmn — inf 


JIM 


6 The notation, ||-||, denotes the 2-norm, or magnitude, of (•). It is defined by: 

IMI = ( uT “)' • 
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for any e € R. Thus, 


V(q,q,t) > 

(3.17,3.18) 


U 


H p + cn v - C~1M 

m2 


f 2 ) liqll 2 + \ (mm - C7A/e 2 ) 


«i||q|| +«2 q 


(3.19) 


where, 


O' i = Mp + CMv ~ CTAff 2 
02 = MM - 


( 3 . 20 ) 


To ensure V^q, q, t) is positive definite, a\ and c*2 must be greater than zero. Solving each inequality 
in terms of e 2 gives: 


£7M f2 < Mp + Wv 

t^M <HM 

=> c 2 7m < MM (m p + c^v) (3.21) 


Solving the quadratic in Equation (3.21) for c yields: 


MM M v 

2 7 m 



( 3 . 22 ) 


Thus, choosing c to satisfy Equation (3.22) guarantees that V (q, q, t) is a positive-definite function. 


Step 2: Boundedness for V. 

Following similar procedures as the above, an upper bound on the Lyapunov Function is: 


1 / . x -,-,,2 1 '^ ,|2 


VXq,q,t) < j (Tp + C7v) ||q|| + pM q + ^7Af ||q|| q 


Using “perfect squares” again, the cross term can be written as: 

2ctm ||q|| q = -C7M (q||q|| -*? -1 ||q||) + C7M»? 2 ||q|| 2 + C7M*? 


(3.23) 


-2 


< c-iMV 2 liqll 2 + cjmv 2 ||q 


-2 ||~ 


(3.24) 


for any 77 6 R. Thus, 


v (q, q ,t) < 1 (7p + C7v + cn/Mn 2 ) l|q|| 2 + 1 ( 7 M + n MV 

(3.23,3.24) 2 V / 2 \ 

< P\ liqll 2 + Pi 


-A ll~ 


(3.25) 
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(3.26) 


where, 

A = 7p + C7v + c~tMV 2 
fa = 7a/ + C7m»T 2 

Noting that A and A sue guaranteed to be greater than zero, Equation (3.25) shows that V (q, q, t) is 
uniformly bounded if the position and velocity errors, q and q, are uniformly bounded. 

Assuming that ||q(t)|| and ||q(f)|| uniformly bounded, the lower bound on V(q,q,t), leads 
to the following: 


m ,q,t) > aillqWII 2 

( 3 . 19 ) 

=► > sup||q(t)|| (3.27) 

where 

v = sup V(q,q, t). (3.28) 

Step 3: Negative-Semi-Definiteness of V. 

Using a particular representation for the Coriolis and centrifugal matrix, C(q, q), and the identities 
in Equations (3.31-3.34), this section proves that F(q, q, t) is never positive, showing that V(q, q, t) 
never increases. Thus if the initial trajectory errors are bounded, they remain uniformly bounded. 


Step 3a: Expanding V . Using the definition for trajectory error in Equation (3.2) to expand q, and 
equations of motion in Equation (3.14) to substitute for M(q)q, and the control law in Equation (3.1) 
to substitute for r, the time-derivative of the Lyapunov Function in Equation (3.9) expands to: 

V (q. q, 0 (3 2 = 14) q T (M(q)q d - T + C(q, q)q + G(q) + ^M(q, q)q + K P q) 

+cq M(q)q 

+cq T (M(q, q)q + K^q + M(q)q d - r + C(q, q)q + G(q)) 

( = } q T (“C(q, qd)qd - Kvq + C(q, q)q + ^M(q, q)q [j 

. x 

+cq M(q)q 

+cq T (M(q, q)q - C(q, q d )q d - K P q + C(q, q)q) 

= — cq r K p q — q^ (K^ — cM(q)) q 


(3.29) 
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+q T C(q, q d )q d + C(q, q)q + ^M(q, q)q) 

+cq T (M(q, q)q - C(q, q d )q d + C(q, q)q) ( 3 . 30 ) 

Choosing a “good” representation of C(q, q) will help further simplify this expression. 


Step 3b: A Representation of C. Begin by first defining a matrix, such that 

*jr / \ dM (q) T ( 331 \ 

Md( q,y) = 2^ dq , y^i (3.31) 

where e» € R n is the ith unit vector. Following are some useful identities for M£)(q, y) 7 : 


Identity h 

«y)=M c( q,y) 

dq 

(3.32) 

Identity 2 : 



M(q»q)y = Mi>(q,y)q 

(3.33) 

Identity 3: 

Mp»(q,y)z - M£)(q,z)y 

(3.34) 

Now choose C(q, q) 

to be the following 8 : 



C(q,q) = Mp»(q, q) - ^M^(q,q) 

(3.35) 


Appendix A.2 shows that this is a valid choice by satisfying the condition in Equation (3.15). Using 
this representation in the Lyapunov derivative of Equation (3.30) yields: 

^(q>q><) (3 3 3 = 3 35 ) -cq r Kpq - q T (Kv - cM(q)) q 

T 

-(q + cq) C(q,q d )q £f 

+q T ^M^Cq, q)q + Mp(q, q)q - ^M£(q, q)q) 

+cq T ^M/)(q, q)q + Mi>(q, q)q - £(q, q)q) 

- -cq T Kpq - q (Ky - cM(q)) q - -q Mp>(q, q d )q 

(3.2,3.343.35) 2 

+cq T ((M^(q,q d ) - M D (q,q d )) t*- ^M£(q,q)q) (3.36) 

7 See Appendix A. 1 for the proofs. 

8 This representation does not satisfy the skew-symmetric property required by the sliding-mode adaptive controller [37]. 
See Appendix A 2 for more details. 
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Step 3c: Upper Bound for V . This step shows that the upper bound for V (q, q, t ) is never positive 
for proper choices of Kp and Ky. First determine the upper bound on the last two terms of 
Equation (3.36): 


1 . t 

— -q Mfl(q,q rf )q 


(3.31) 


1 ~ T / Y' ^M(q) T \ 


, 1 i 2 

< j 7173 q 


where 


7i = sup ( £ 
q \i=i 

73 = sup |q<f| 

t 


0M(q) 




,) 


(3.37) 

(3.38) 

(3.39) 


Similarly, 


cq r ^(Mp(q, q d ) - M/)(q, qd)) q - ^M£(q, q)q^ 


. 1 . 2 
< 2c7i 7 3 ||q|| q + rC7i ||q|| q 


< C7i73^ 2 ||q|| +C7i7 3 ^ q + -C7! ||q|| q 

(3.18) 2 


1 


(3.40) 


Note that the “perfect squares” expression similar to Equation (3.18) is used once again to eliminate the 
cross term, 2 c 7 i 7 3 ||q|| llqll. Combining these results gives: 


V(q, q, t) < -A, ||q|| 2 - A 2 $ * + ||q|| ^ 

(3.36,3.37, 3.40) 2 


where 


Ai = c (/ip - 7i' 73f 2 ) 

^2 = - C7M - 7i73 


(HI) 


(3.41) 

(3.42) 

(3.43) 


Both X\ and A2 must be positive. This leads to the 

condition: 

1 C7173 

✓ 

e<-^~ 

7173 

2 li v - n M ~ 2 7l72 

s 

=» 

< 

(nv ~ nM ~ ^7172 

=> c 

< 

Mp (/*« - 57173) 
M P 7M + |7 i73 


(3.44) 
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Since c > 0, the last inequality shows Kv must be chosen large enough to satisfy: 




(3-45) 


to ensure that V (q, q, t) in Equation (3.41) has some negative definite terms. 

Now assume that |Jq|| and q are bounded such that V defined by Equation (3.28) exists, choose 
K v large enough such that: 


^2 


Xu 1 - 


Then 


1 

— ( 

2 


\ >0 

(3-46) 

> 0 
( 3 . 27 . 3 . 46 ) 

(3-47) 

2 — A 2 |q | <0 

(3-48) 


The upper bound on V (q, q, t) can now be written as: 

^(q,q»<) < -Ai||q 

( 3 . 41 , 3 . 47 ) 

Thus given Kv large enough, V (q, q, f) is never positive, so V (q, q, t) is never increasing, such that 

V = m*o),q(*o),*o). (3-49) 


This in turn implies, from Equation (3.19), that the trajectory errors, q and q, are uniformly bounded. 

If the initial errors, q(to) and q(£o)> are small enough — so V is small enough — then the assumption 
in Equation (3.46) can be satisfied easily. 


Step 4: Stability 

Application of Barbalats theorem at this point shows that if the trajectory errors remain bounded, then 
they, in fact, also tend to zero. The theorem is stated as follows [6]: 


Barbalats Theorem 

If W(t) is a real function of the real variable t, defined and uniformly continuous for t > 0, and if 
the limit of the integral 

lim f W(t')dt' 

t-*oo J o 

exists and is a finite number, then 


lim W(t) = 0. 

t—*oo 
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In other words, if the integral of a continuous function approaches a limit and does not grow without 
bound, then W must approach zero, because it cannot continue to add to the integral. 


To show stability of the controller, let 

W(t) = ^\\ q|| 2 + A 2 ||4|| 2 (3.50) 

such that 

vXq.q.*) < -w(t). (3.5i) 

( 3 . 48 ) 

Integrating both sides of Equation (3.51) from 0 to t, yields upon rearranging, 

t mt') dt' < V(q(0), m, 0) - F(q(t), 4(0, t ) (3.52) 

J 0 

Since V (q(0), q(0), 0) is bounded, and V (q, q, t) is nonincreasing and bounded below, it follows that 

lim f W(t')dt r < oo (3.53) 

t—oo J 0 

Also, since q<j is chosen to be bounded, and since rigid-body dynamics dictate that q is bounded for 
bounded input forces and torques, W(t ) is bounded, which implies that W (t) is uniformly continuous. 
Applying Barbalat’s Theorem, therefore, gives 


lim Wit) = 0 

t— too 


llqll - o 



This proves the asymptotic stability of the nonadaptive controller. 


(3.54) 


3.3.3 Adaptive Controller Stability 

As the previous section showed, modifying the Lyapunov function to include the parameter-estimate 
errors, and applying the adaptive update law of Equation (3.7), makes the above proof equally applicable. 
Thus this section has also proved the asymptotic stability of the adaptive controller. 

Finally, it should be noted that the above proof used rather loose bounds to derive requirements on 
the gains, c, Kp, Ky. These requirements are sufficient, but not necessary, so one can use them for 
initial design. In practice, the gains that achieve the best controller and adaptation performance may 
not meet these criteria. 



Chapter 4 


Adaptive Control Extensions 


The Bayard and Wen class of adaptive control algorithms is applicable in a relatively restricted realm — 
single-arm joint control. This chapter describes the first steps in extending the adaptive control 
algorithm to fctt£-spacc control: 1) extension to utilize equations of motion developed using generalized 
speeds to ease the design procedure and 2) extension to endpoint control. These extensions provide the 
essential building blocks for developing the task-s pace adaptive controller in Chapter 5. 

Section 4.1 extends, in a limited fashion, the adaptive algorithm to handle the more generalized 
modelling method using generalized speeds. This extension benefits controller implementation by 
simplifying the terms that appear in the equations of motion. The approach is straightforward and, 
except for a transformation from actuator torques to generalized forces, involves little change from the 
original adaptive controller. 

Section 4.2 further extends the algorithm to handle endpoint control. This extension utilizes the 
end-effector Jacobian matrix for transforming endpoint motions in Cartesian space to variations in 
the generalized coordinates and generalized speeds. Once again, the changes to the original controller 
structure are minor; this section furnishes a summary of the differences. 

Section 4.2.3 offers an oudine and the detailed stability proof for the new endpoint adaptive 
controller. The Lyapunov function must be altered for endpoint control, and includes the Jacobian 
matrix. The proof follows the same steps as the original proof by Wen, with an additional stability 
requirement that the manipulators must stay away from kinematic singularity. Since endpoint control 
perse also makes this requirement, this condition does not pose a serious limitation to the new endpoint 
adaptive control algorithm. 
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4.1 Extension to Generalized Speeds 

Appropriate choices for generalized speeds can simplify the terms in the equations of motion for a 
system. This aids implementation by reducing the number of calculations 1 . For instance, the two-link 
arm example in Chapter 2 illustrated that for a planar robotic system with revolute joints, it is beneficial 
to choose generalized speeds that correspond with the absolute rotation rates of the links, rather than 
the joint rates. This section, therefore, extends the adaptive controller given by Equation (3.4) and 
Equation (3.7) to this useful, albeit limited, case. 

For generalized speeds chosen to correspond with absolute angular rates for the rotational degrees of 
freedom, the generalized speeds and the joint rates are related by a constant matrix of zeros and ones 2 : 


U = Wq (4.1) 

where W is the n x n constant matrix. The new joint-space adaptive control law can be expressed as: 

F = M\q)u d + C'(q,u d )xi d + G'{q) + K v u + W- T K P q (4.2) 

r = W t F (4.3) 

or in terms of the parameter vector: 

F ( 29 ) Y/ ( q ’ Ud ’ Urf ’ + KkS + w_r Kpq (4.4) 

= Y'(q,\i d} u d ,xi d )9 + KvVL + W- T Kpq-Y , (q,u d ,u d ,u d )9 (4.5) 

The corresponding new parameter update law is: 

0 = rY' T (q, u d> u d, ii d ) (u + cW r q) (4.6) 


where the / notation is used to distinguish the system equations from those derived not using gener alized 
speeds. That is, M'(q), C'(q,u), G'(q), and Y'(q, u^, u<i, u</) are the matrices defined by the 
generalized equations of motion, 

F = M'(q)u + C'(q, u)u -I- G'(q) = Y'(q, u, u, u)0. (4.7) 

Figure 4.1 shows the block diagram of the new controller. Application of the control law, 


'The example in Chapter 2 demonstrates this. 
2 See the example given by Equation (2.6). 
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Figure 4.1: Block Diagram of the Adaptive Controller using Generalized Speeds 

This controller allows the use of a system model that was developed with generalized speeds. 
The controller structure is almost identical to the original controller, Figure 3 A, with the 
exception of the transformation block , W, to bring generalized forces into actual joint torques. 


Equation (4.4), results in a mathematical vector representing the desired generalized forces to apply to 
the system. Implementation, therefore, requires the transformation of this vector back into actuator 
torques. Other than this transformation, there is little change from the original adaptive controller. 


4.1.1 Transforming Equations of Motion 

Examining the relationship between the two forms of equations of motion defined by Equation (2.1) 
and Equation (2.3) is useful for simplifying the stability proof. 

Differentiating the generalized speeds in Equation (4.1) with respect to time yields: 

u — Wc\ (4.8) 

Substituting Equation (4.1) and Equation (4.8) into the equations of motion using qs results in: 

r = M(q)q + C(q, q)q + G(q) 

\Z.j) 

= M(q)W _1 u + C(q, W _1 u)W _1 u + G(q) (4.9) 

( 4 . 1 , 4 - 8 ) 
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Premultiplying both sides of Equation (4.9) by W T gives: 

W ~ t t = W- r M(q)W- , u + W- T C(q,W- , u)W" 1 u + W" r G(q) (4.10) 


Matching terms of the generalized equations of motion in Equation (2.1) with those in Equation (4.10) 
yields: 


F = W - r r 
M'(q) = W~ T M(q)W -1 

(4.11) 

C'(q,u) = W- T C(q,W' 1 u)W- 1 
G'(q) = W“ r G(q) 

It also follows from Equation (4.10) that, 

Y'(q,u,u,u)0 = W”^Y (q, W _1 u, W _1 u, W^u)# (4.12) 


The equations in (4.11) and (4.12) represent the transformation between the two forms of equations 
of motion and the transformation between generalized forces to actuator torques. 


4.1.2 Stability Proof 

The stability proof for this case is rather straightforward. Substituting the transformations in Equa- 
tion (4.1 1) into the control and adaptive laws of Equation (4.4) and Equation (4.6), results in exactly the 
original Bayard and Wen control and adaptation laws 3 . The same Lyapunov Function, Equation (3.10), 
therefore, can be used to prove stability. 

For completeness sake, the Lyapunov Function and its derivative can be written in the / notation 
and in terms of generalized speeds as: 

V(q,u,0 = ^u r M / (q)u+ ^q r (Kp + cKv)q + cq r W r M , (q)u+ ^ T r _1 6» (4.13) 

and 

V' (q, u, i) = u T M'(q)u+ ^u r M'(q,u)u + q T (Kp + cKv)q 

+cu r M'(q)u + cq T W T M'(q, u)u + cq T W T M'(q)u 

+0 T T~ l 0 + 6> T Y' T (q, uj, Ud, Ud) (« + cWq) (4.14) 

Using the transformation equations also will reduce these to the original Lyapunov function and its 
derivative. 


3 See Appendix A.3 for the proof. 
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4.2 Extension to Endpoint Control 

While endpoint control can benefit a fixed-base industrial robot performing precise, dextrous manip- 
ulation, it is crucial for a mobile robot that needs to perform acquisition before any manipulation. 
With no fixed base from which to reference the location of the parts it is to acquire, the mobile robot 
controller must use feedback direedy on the manipulator endpoint positions relative to the part. The 
space robot capturing a free-flying satellite, for example, must use endpoint control to track the grasp 
points on the satellite before capture. 

Hence, this section further extends the adaptive control algorithm to endpoint control. The desired 
trajectories a manipulator must follow are given in terms of endpoint Cartesian coordinates. The 
adaptive law also updates the parameters based on the endpoint tracking errors. Although at this point 
this extension is still applicable only to a single- manipulator robot, the results can be quite useful for 
later extension to the full cooperating-manipulator space robot. 


4.2. 1 Endpoint Jacobian 

The extension from joint control to endpoint control utilizes the Jacobian matrix. Figure 4.2 illustrates 
the relationship between the endpoint and joint positions. In general, the Jacobian matrix arises 
when differentiating mathematical vector fields 4 . It is composed of the partial derivatives of the vector 
function. In robotics, the term , Jacobian, has taken on a more specific meaning, namely, the relationship 
between joint rates and the manipulator end-effector velocity (a physical vector) and orientation rates. 

Simple kinematic analysis gives the endpoint position and orientation as functions of the joint 
angles, represented by: 

x = k(q) (4.15) 


where x € R m contains the endpoint position and orientation, and k : R n — ► R m are the kinematic 
equations. When m = n, the robot is said to be nonredundant, and when m < n, the robot is 
redundant. 

Taking the time-derivative of both sides of Equation (4.15) gives: 


* = flfc(q) ^ + dk (q) 


0q 


dt 


(4.16) 


4 Not to be confused with the physical position (x) and velocity (i) vectors which are of course also the centerpiece of 
Figure 4.2. 
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Figure 4.2: Cartesian Endpoint Position and Velocity 

q\ and qj represent the shoulder and elbow joint angles, x is the position vector of the 
manipulator endpoint and i represents die endpoint velocity vector. 


Defining the Jacobian to be: 


and a time-dependent term to be: 


J(q) = 


flfc(q) 
9 q 


J*(q) 


flk(q) 

dt 


yields the endpoint Jacobian relationship for robotics: 


x = J(q)q + J t (q) 


(4.17) 


(4.18) 


(4.19) 


When there are no prescribed 5 motions in the system, J*(q) is zero, which yields the more common 
relationship: 


X = J(q)q 


(4.20) 


5 A prescribed motion is a motion that the system is forced to undergo in response to unmodelled external forcing functions. 
In robotic systems, where all forces are modelled, there are no prescribed motions. 
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The inverse Jacobian relationships are: 


q = J '(q)x- J 1 (q) J t(q) ( 4 - 21 ) 

q = J -1 (q) (x- J(q,q)J _1 (q)x- J t (q)) (4.22) 

To make use of generalized speeds, additionally define a transformed Jacobian, J^q), as: 


J'(q) = J(q)W-> 

such that the following are true: 

u = Wq 

x = J , (q)u + J((q) 

u = J ,_1 (q)x — J ,- 1 (q)Jf(q) 

u = J ,- 1 (q) (x — J^q, u)J ,-1 (q)x — j{(q)) 


(4.23) 


(4.24) 

(4.25) 

(4.26) 

(4.27) 


Example 

The two-link arm example illustrates that using generalized speeds can also simplify the terms in the 
Jacobian. The physical position vector for the endpoint can be written as (See Figure 4.2): 


x = ( l\ cos(gi) + hcos(qi + q 2 ) + x 0 ) aj + ( l\ sin(q\) + Z 2 sin(gi + q 2 )) a 2 (4.28) 


where ai and a 2 are mutually orthogonal unit vectors, and xq is a constant offset of the shoulder from 
the origin of the coordinate system. Taking the partial derivative with respect to q gives the Jacobian: 



-f 1 sin(gi ) - h sin(gi + q 2 ) -Z 2 sin(gi + q 2 ) 

1 1 cos(<?i ) + l 2 cos(q\ + q 2 ) h cos(?i + q 2 ) 


(4.29) 


where it has been expressed in the coordinate system aligned with aj and a 2 . The transformed Jacobian 
for use with generalized speeds, utilizing W from Equation (2.6), is: 


J'(q) 


(4.23,2.6) 


-Zi sin(q-i ) -Z 2 sin(gi + q 2 ) 

l\ cos(gi) h cos(gi + q 2 ) 


(4.30) 


Because there are no prescribed motions, Jt(q) and J*(q) are zero. 
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4.2.2 Endpoint Adaptive Control 

Using the transformed Jacobian matrix defined above, the endpoint adaptive control law can be written 


in terms of generalized speeds as: 

F = M'(q)u<f 4- C'(q, + G'(q) + J' T (q) (Kv x x + Kp x x) (4.31) 

r = W r F (4.32) 

or in terms of the parameter vector: 

F ( 29 ) Y ^ q ’ Ud ’ Ud ’ (Kv*x + Kp x x) (4.33) 

= Y'(q, u<f, Ud, u i)d + J' T (Kk x x + Kp^x) - Y'(q, u <*, Ud, u<i)0 (4.34) 

The corresponding adaptive update law is: 

0 = rY ,T { q, Ud, Ud, Ud)J' _1 (q) (x + cx) (4.35) 

In addition, the desired generalized speed and its derivative are given by: 

u <* J,_1 (q) J J(q) (4.36) 

( 4 . 26 ) 

J'"'(q) (*j- j'(q.u)J'"'(q)x 
- j'(q,u)J'- | (q)J!(q) - ji(q)) (4.37) 


The key differences between the new endpoint adaptive controller and the joint controller can be 
described as: 

• Endpoint-trajectory tracking. The controller tracks endpoint trajectories, because it performs 
feedback direedy on the endpoint tracking error. The adaptive parameter update is also based on 
the endpoint tracking error. 

• Jacobian calculations. The Jacobian calculation blocks are the only real structural changes in 
the controller. They are needed to convert the desired endpoint trajectories into equivalent 
desired joint-space trajectories for use by the inverse-dynamics feedforward block. The Jacobian 
also translates the feedback forces, expressed in Cartesian space, into the mathematical space 
represented by the generalized forces. Finally, the adaptive update block also uses the Jacobian to 
map the tracking errors to the joint space, in which the adaptive model is derived. 
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Figure 4.3: Block Diagram of the Endpoint Adaptive Controller 

The endpoint adaptive control tracks endpoint trajectories by performing feedback direcdy on 
the endpoint errors , x. Note that the adaptive parameter update is also based on the endpoint 
tracking error. This controller structure is once again very similar to the original controller, 
Figure 3.1. The only additions are the Jacobian blocks that transform the endpoint quantities 
into equivalent joint-level quantities . The transformation block, W, is still required to bring 
generalized forces into actual joint torques. 


• Torque transformation. Since the control law is given in terms of generalized speeds, the transfor- 
mation block, W, is still required to transform the commanded force into actuator torques. 


4.2.3 Stability Proof 

The stability proof involves choosing a new Lyapunov Function to represent the “energy” in terms of 
the endpoint tracking errors and then showing that it goes to zero, implying that the tracking errors also 
go to zero. In doing so, the proof places an additional requirement on the controller: the manipulator 
must remain away from geometric singularity, which equates to the matrix inverse of the Jacobian being 
bounded. This is a very natural requirement, since the controller becomes ill-behaved near kinematic 
singularities; it is also a requirement whenever one implements a Cartesian-space controller. In practice, 
kinematic singularity is easy to detect 6 . If it cannot be avoided, the controller can change control modes 

^Typically, kinematic singularities occur when two or more degrees of freedom of the manipulator “line up”, where the 
manipulator loses degrees of freedom. For the two-link manipulator, kinematic singularity occurs with the arm straight out 
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to bring the manipulator into a more favorable configuration. 

This proof starts with the nonadaptive controller by choosing the Lyapunov Function and calculating 
its derivative. It then follows steps similar to that of the original proof in Chapter 3: 

1. Show that the Lyapunov Function, V(x y x y t), is positive definite with the proper choice of c. 
Determine the appropriate bounds on c. 

2. Show that the Lyapunov Function is bounded if the velocity and position errors are bounded. 

3. Show with the following steps that the time-derivative of V^x, X, t) is never positive: 

(a) Use the definitions for trajectory errors, equations of motion, and the control law in 
Equations (3.2, 2.1, 4.33) to expand V(x y x y t). 

(b) Use the Jacobian relationships, Equation (4.25) and Equation (4.26), to simplify terms in 

V(x y x y t). 

This reduces the form of V (x, x, t) to the same form as in the original proof. Thus, the rest of 
the stability proof follows the original, with minor changes to the necessary bounds. 

This proves that x and x tend to zero as t — ► oo, showing stability. 

4. The Lyapunov Function is then modified to include adaptation. The application of the adaptive 
update law, Equation (4.35), reduces the stability proof for the adaptive algorithm once again to 
the nonadaptive case, where stability has been shown already. 

Endpoint Lyapunov Function — Nonadaptive 

The new Lyapunov Function for the system operating the nonadaptive endpoint controller is: 

V(x,Si,t) = ^x T (j ,_r (q)M , (q) J' _1 (q)) x + ^x T (K Px + cK Vx ) x 

+cx T (j' _T (q)M'(q) J' _ 1 (q)) k (4.38) 

The first two terms represent the “energy” in the endpoint velocity and position errors. They are squared 
terms, so they are always positive. The third term is a cross product term between the velocity and 
position errors — this term is not guaranteed to be positive, so the positive-definiteness of Equation (4.38) 


such that the links lie along the same line. In this situation, the endpoint of the arm can trace out only an arc — defined by the 
shoulder angle and the total length of the arm — which is only one degree of freedom. 
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must be shown. The scalar, c > 0, is a weighting on the cross term — c is the same weighting that 
appears in the adaptive update law in Equation (3.7). 

Using the inverse Jacobian relationship, Equation (4.26), and replacing x with u, the Lyapunov 
Function also can be expressed as 7 : 

V (x, x, t) = ^u r M'(q)u + ^x T (Kp x + cKv x ) x + cx T J' -T (q)M'(q)u (4.39) 

( 4 . 26 ) Z Z 

Noting that M'(q), Kp, and are symmetric, the time-derivative of the Lyapunov Function can be 
written as: 

• • * 1 • * T 

V(x,x,t) = u r M'(q)u + -u r M'(q,u)u + x (Kp* + cK^x) x 

+cx T J'~ T (q)M'(q)u + cx T j' -r (q, u)M'(q)u 

+cx T J ,-r (q)M'(q, u)u + cx T J , ~ r (q)M , (q)u (4.40) 


Positive-Definiteness of V 

Using the first expression for V (x, X, t) in Equation (4.38) and following the same progression as in 
Step 1 of the proof in Section 3.3.2 show that: 


where, 


y(x,x,t) > c*i ||x|| + a>2 

( 4 . 38 , 3 . 19 ) 


= n p + cn v - c-fffe 2 


« - %- c %‘ 

with the following definition of the limits: 


-2 


MM = inf &min (M'(q)) 
M J — inf o’mtn (j (*l)) 

qgs 


7 Note that: 


such that. 


X = Xd — X 

= (J'(q)ua + Jl(q)) - (J'(q)“+J<(q)) 

= J'(q)(ua-u) 

*=J'(q)« 


(4.41) 


(4.42) 
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Mp 

A 

&min (K/>) 


A 

^min (K,/) 

1M 

A^ 

sup &rnax (M'(q)) 

q 

1J 

A 

sup (Tmax (J'(q)) 

7p 

_A 

&max (K/>) 

lv 

A 

&max (Ky) 


5 represents a set of regions around singularities. That is, the limits for J'(q) are defined for manipulator 
configurations that exclude finite regions around singularities. For V^x, x, t) to be positive definite, 
both ai and a 2 must be positive, which puts a condition on c: 


t-th-ii 



) ) 


(4.43) 


The additional terms, ji j and 7 j, are the main changes these bounds. It is clear from Equation (4.43) 
that for c to have a non-trivial solution, the manipulator must remain away from singularity such that 

¥=> 0 . 


Boundedness for V 


Following the same procedure as Step 2 of the proof in Section 3.3.2 shows that: 


^(x,x,t) < 

(4.383.25) 


A I|x || 2 + A 


(4.44) 


where, 


A = 7 P + erfv + c^fr) 2 

A = Vf + cVf-n - 2 

t l J 


(4.45) 


It is again clear that if the robot remains away from singularity such that fij / 0 , then V(x, x, t ) 
remains bounded. 


Negative-Semi-Definiteness of V 

Using the definition for trajectory error, Equation ( 3 . 2 ), to substitute for u, and using the equations of 
motion, Equation ( 2 . 1 ), to substitute for M'(q)u expands the Lyapunov derivative in Equation (4.40) 
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to: 

V(x,k,t) (440 = 2 1} u r ^M'(q)u<f - F + C'(q,u)u + G'(q) + ^M'(q,u)u 

+J ,T (q)Kp I x) + cu r M'(q)u + cx T j'~ T (q, u)M'(q)u 
+cx T J'~ T (M'(q, u)u + M'(q)u<j - F + C'(q, u)u + G'(q) 

+ J' r Ky*i) (4-46) 

where the Jacobian relationship, Equation (4.25), is used for the K p x term. Substituting the control 
law in Equation (4.31) — assuming an ideal model — for F yields: 

V(x,k,t) = uu T f-C'(q,u d )u d - J ,T (q)KvxX+ C'(q,u)u+ ^M'(q,u)u) 

(4.31) \ z / 

+cu T M'(q)u + cx T j ,_T (q,u)M'(q)u 

+cx r J ,_T (q) (M'(q, u)u - C^q, Ud)u d - J ,T K Pi x + C'(q, u)u) 

= -cx T K Px x - x T (K Vx “ cJ'~ r (q)M'(q) J' _ 1 (q)) x 
+cx r j'- r (q ( u)M'(q)u 

+u T (-C'(q, Ud)u d + C'(q, u)u + ^M'(q, u)u j 

+cx r J' -r (q) (-C'(q, u d )u d + C'(q, u)u + M'(q, u)u) 

= -cx T K Px x - x T (Kvx - cJ ,_T (q)M'(q)J ,_1 (q)) x 

(4.11,4.26) v ' 

+cx T j /_T (q,u)M'(q)J' _1 (q)x 

+^ r j /_T (q) W T (— C(q, qrfjqrf + C(q, q)q + ^M(q, q)qj 
+cx T J'~ T (q)W T (— C(q, qd)q<j + C(q, q)q + M(q, q)q) (4.47) 

The last two terms of Equation (4.47) are almost identical to those in Equation (3.36), except for the 
J'~ T (q)W T premultiplier. Assuming once again that the manipulator stays away from singularity and 
noting that W is a constant matrix of zeroes and ones, the development in the original proof can be 
used once again to state: 

V(x,x, t) < -Xi ||x|| 2 - A 2 llxll 2 + 11*11 ||x|| 2 (4.48) 

(3.41) 11 11 2 VJ 11 11 

where 

74 = sup |xd| 


(4.49) 
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A 

Tj-» = 

sup (Tmax ( J 

i'-'(q)) 

(4.50) 

q ts x 


a, a 

( 71 

c ( Mp 

V 

(7. + 1j-i)f 2 ) 

(4.51) 

a 2 a 

(Vt; ~ cn/M 

+ G + «" 2 )) 

(4.52) 


The dependence on Jj-\ comes from the extra term in the Lyapunov derivative involving j' _T (q, u). It 
can be argued again that if the manipulator does not go through singularity, then J'~ ! (q) is continuous, 
and thus its derivative is bounded. Requiring both As to be positive gives the requirement: 


K Vx > (74+7J-.) 

( 3 . 44 , 3 . 45 ) 2 Hj J 


Choosing Kv' x large enough such that: 

A i(iy >0 
2 / 


gives: 


A 2 = A 2 - ||*|| > 0 

2 HJ 


which yields the upper bound on the Lyapunov derivative: 


(4.53) 


(4.54) 

(4.55) 


V(x,x,t) < - A, ||x|| 2 - A 2 |x| 2 < 0 (4.56) 

( 4 . 48 , 4 . 55 ) 11 11 

Thus, the Lyapunov Function, V^x, x, t) is never increasing, showing that if the initial errors are 
bounded, they remain bounded. Again applying Barbalat’s Theorem shows that x and x tend to zero 
as t — ► oo, proving stability. 


Endpoint Lyapunov Function — Adaptive Case 

The Lyapunov Function for the system utilizing the adaptive endpoint controller follows the example 
of the original proof by adding a term representing the energy in the parameter estimation errors: 

Vi(x, x,t) = V(x, x, t) + ^9 T r~ l 0 (4.57) 

Using the adaptive control law in the form of Equation (4.34) to substitute for F after expansion (see 
Equation (4.46)) and taking the time derivative of Equation (4.57) gives: 

V\ (x,x,t) = V(x,x,t) + 9 T r~'6 + d T Y rT (q,\i d ,u d ,u < i)J'~ l (q) (i + cx) (4.58) 
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Noting again that 9 = —6 and substituting the adaptive update law, Equation (4.35), eliminate the 
last two terms, reducing V\ (x, x, t) to V (x, x, £). Employing the proof developed for the nonadaptive 
case proves the stability of the endpoint adaptive controller. 



Chapter 5 


The Task-Space Concept 


The extension of the adaptive controller to endpoint control in Chapter 4 provides the essential 
components for further extensions to task space. This chapter presents the task-s pace concept in terms 
of a generalized Jacobian. It also shows that, utilizing the generalized Jacobian approach, the new 
endpoint adaptive control algorithm can be extended to task space with almost no modifications. 

This chapter takes full advantage of the power in the generalized forces and generalized speeds 
concepts found in Kane's dynamical equations [15] to derive the expression for the generalized Jacobian. 
Section 5.1 presents the taskspzct adaptive control law for single-manipulator robots, showing that 
endpoint control is just a special case of the tasks pace adaptive control. Section 5.2 develops and 
formalizes the relationships between two different sets of generalized speeds chosen for a system. It also 
examines the relationships between the two sets of generalized active forces and equations of motion 
associated with the choices of generalized speeds. These generalized relationships show that the time 
derivative of the tasks pace control vector is equivalent to a set of generalized speeds, which leads to an 
intuitive validation of the taskspzot adaptive controller. 

The chapter concludes with several sections providing specific examples of tasksp&cc control 
objectives: 

• Noninertial reference-frame control This control mode is important for mobile robots that must 
rely on local sensing. If the robot base rotates or accelerates during manipulation, the local 
sensors will be providing sensing in a noninertial frame; the controller must of course take this 
into consideration. 


48 



49 


Fuel savings is another benefit of noninertial reference frame control for space robots. The 
controller decouples the feedback of the noninertial frame control of the manipulators from the 
feedback of the free-flying base control. Thus manipulator tracking errors during regulation do 
not cause the thrusters to fire. The thrusters will activate only in response to base regulation 
errors 1 . The implication is that the manipulator control gains may be increased to improve 
manipulator tracking without incurring unnecessary thruster firings during regulation. 

• Single-manipulator space robot control This example describes how task-s^zct control may be 
utilized for control of a space robot. This example uses a single-manipulator space robot, because 
the task-spzct adaptive controller for multiple-manipulators has not been presented yet 2 . The 
characteristics of space robot control remain the same regardless of the number of manipulators. 

• Control subject to motion constraints. Motion constraints arise when a manipulator comes into 
contact with its environment. Understanding the issues in control of constrained motion is 
critical to understanding multiple-manipulator control. 

• System-momentum control. System-momentum control is another control mode for providing 
fuel saving control for space robots. Maintaining constant system momentum during local 
manipulation ensures that the manipulators will compensate appropriately for the reactions in 
the base to prevent thrusters firing, conserving precious fuel. System-momentum control has been 
presented by Umetani and Yoshida [44] and Koningstein [19], and this example demonstrates 
how task-spzct control may be used to achieve the same control mode. The added complexity 
of deriving symbolically the momentum equations, however, may make this control mode too 
cosdy to implement, in a given application, compared to the noninertial-reference-frame control 
for minimizing fuel usage 3 . 

• Redundancy Management. The task- space controller can be employed to control a limited class of 
redundant manipulator systems. The difficulty typically arises because the endpoint Jacobian is 
not square: There are more degrees of freedom in the robot than there are in controlled states of 
the end effector. Since a nonsquare Jacobian cannot be directly inverted, much of the redundancy 

l There is full coupling in the feedforward portion of the controller. 

2 Multiple-manipulator control will be presented in Chapter 6. 

3 The complexity should be examined for each given application to determine the feasibility of implementing system- 
momentum control. 
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research has concentrated on the choice of appropriate, but computationally intensive, generalized 
inverses for the Jacobian. 

Tasksp&ce control offers another solution by including auxiliary control objectives in the task - 
space control vector to generate a square generalized Jacobian. For example, the auxiliary control 
objective may be the height of an elbow in a 7-degree-of-freedom manipulator; the elbow height 
may be controlled directly to avoid obstacles while simultaneously controlling the end-effector 
position and orientation. Thus if suitable and valuable auxiliary control objectives can be added, 
task-s pace control offers a good alternative to the standard redundancy controllers. 

Armed with the capabilities developed in this chapter, Chapter 6 will provide the final extension to 
multiple-manipulator task-space robot control. 

5.1 Task-space Adaptive Controller — Single-Arm Case 

For a robotic system, choose y task as any set of tasks pace quantities to be controlled, physical quantities 4 
that may be expressed as n linearly independent functions of the generalized coordinates, q: 

ytask _ h(q) (5.1) 

where h : lR n — ► lR n . Its time derivative can be expressed as: 

y‘- fc = W'(q)q + W;(q) (5.2) 

where W'(q) is a n X n matrix and W{(q) is a n vector of functions of q and time t 5 . Also choose 
for the system a set of generalized speeds, u, related to the generalized coordinates by: 

u = W(q)q + W*(q) (5.3) 

4 As will be shown, these physical quantities can include not only linear translations and angular rotations, but any physical 
quantities that can be expressed as functions of the generalized coordinates. Keeping the arms in a high-leverage configuration 
and away from singularity, and minimizing thruster use are two cogent examples. 

5 In particular, 

W(„ = ^> 

and 

a»(q) 

9t 
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where W (q) is a n x n matrix and W*(q) is a n vector of functions of q and time t. Let the equations 
of motion for the robotic system, using this set of generalized speeds, be expressed as: 

F = M'(q)u + C'(q, u)u + G'(q) + f t = Y'(q, u, u, u )0 + f t (5.4) 

where ft is added to represent the effects of prescribed motions. 

Now define the generalized Jacobian 6 as: 

3{q) = W , (q)W _1 (q) (5.5) 


and a time-dependent vector, *Tt(q), as: 

Jt{ q) = (w'(q) - W , (q)W -1 (q)Wt(q)) (5.6) 


such that, 

y task = J( q)u + J t ( q) (5.7) 

Then the task-s pace adaptive control law is: 

F = M'(q)u <1 + C'(q,u < i)u ( , + G , (q) + f f + ^ T (q) (Ky v y + K Py y tasfc ) (5.8) 
r = W(q) T F (5.9) 

where y task = — y task and y iask — y^ 8k — y task . The control law can be expressed in terms 

of the parameter vector as: 

F( 2 = Y'(q, u d , u d) u d )9 + tt + J T { q) + K Fy y taafc ) (5.10) 

The corresponding adaptive update law is: 

d = rY' r (q,u d) u d) u d )J'- 1 (q) (y^ + cy ta,fc ) (5.11) 

In addition, the desired generalized speed and its derivative are given by: 

u d ( = q) (5.12) 

( = J r -Hq)(yd°' fc -^(q.«)^" 1 (q)y t ° 9fc 

- j(q, u)J~ l (q)J t (q) - j t ( q)) (5.13) 

6 The generalized Jacobian presented in this thesis is a more formal definition than that presented by Umetani and Yoshida 
[44]. The generalized Jacobian is generalized, because it relates two sets of generalized speeds, and its terms obviously depend 
on the choice of the sets of generalized speeds. 




52 


Chapter 5. The Task-Space Concept 


The only requirement is that the configuration of the system must be monitored to ensure that J (q) 
stays away from singularity 7 . 



Figure 5.1: Block Diagram of the Tas k-Space Adaptive Controller — Single-Manipulator Case 

The task-space adaptive control tracks task-space trajectories by performing feedback direedy 
on the task-space errors , y task . Note that the adaptive parameter update is also based on the 
task-space tracking error. This controller structure is once again very similar to the original 
controller , Figure 3.1, and almost identjeai to the endpoint adaptive controller Figure 4.3. 

The only changes are the use of the generalized Jacobian blocks that transform the task-space 
quantities into equivalent generalized speeds. The transformation block, W(q), is still required 
to bring generalized forces into actual joint torques . 


Figure 5.1 shows the structure of the task-space adaptive controller. Note the similarity with the 
endpoint adaptive controller of Figure 4.3. 

In fact, comparing the task-space adaptive controller (Equations 5.7-5.13) with the endpoint 
adaptive controller (Equations 4.31^4.37) shows that choosing y task to be the end-effector position 
and orientation 8 , the two controllers are identical. 

The task-space adaptive controller represents a generic controller structure, capable of handling 


7 For the general task-space vector— one that does not correspond only to end-effector position and orientation — the 
singularity points of & (q) can be caused by either kinematic singularity or algorithmic singularity. Kinematic singularity 
occurs when the physical configuration of the system is such that there is a loss of degree of freedom, e.g., when two links are 
aligned. Algorithmic singularities are all other configurations, q, such that rank (<7Xq)) < n * 

®Note that although the planar examples in this thesis do not show the end-effector orientation, a general manipulator 
end-effector configuration is described by its position and orientation. 
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any number of control modes just by switching the task-space control mathematical vector. Switching 
control modes essentially involves only the switching of the generalized Jacobian blocks and the trajectory 
generators to produce trajectories appropriate for the new task-space control quantities. The feedback 
gains also should be changed appropriately for the controlled quantities; but none of the inverse 
dynamics controller, the adaptive update blocks, or the system parameters need be modified. Because 
the system model does not change during control mode switches, there will be no transient response in 
the system. 

5.1.1 Proof 

The stability proof for the endpoint adaptive controller in Chapter 4 is the stability proof for the task- 
space adaptive controller. At each step in the proof, simply replace x with y task f and J'(q) with J (q). 
All that remain are the justification for the definition of the generalized Jacobian and its relationship to 
task-space quantities. 


5.2 Generalized Relationships 

This section establishes the relationships between two sets of generalized speeds, partial velocities, gen- 
eralized active forces, and equations of motion. Utilizing these generalized relationships, Section 5.2.5 
provides additional validation of the task- space adaptive controller — this time on an intuitive level 
showing the similarity between the task- space adaptive controller and the original Bayard and Wen 
controller. 

5.2.1 Generalized Speeds 

Define two sets of generalized speeds as: 

u = W(q)q + W t (q) (5.14) 


and 


u' = W'(q)q + W'(q) 


(5.15) 
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Solving for q in Equation (5.14) and substituting into Equation (5.15) gives 9 : 

u' = W'(q)W-'(q)u+ (w'(q) - ^ , (q)W- , (q)W t (q)) (5.16) 

where the terms in the parenthesis are independent of both sets of generalized speeds, u and u'. 
Making the generalized Jacobian definitions, 


^(q) = W'(q)W-*(q) 

(5.17) 

and a time-dependent vector, *T*(q), as: 


Jt{ q) = (W'(q) - W'(q)W- 1 (q)W t (q)) 

(5.18) 

Equation (5.16) becomes: 


u' = j'(q)u+ J"t(q) 

(5.19) 


Comparing u' in Equation (5. 1 5) with the time-derivative of the rank-space vector in Equation (5.2), 
shows that y tatk is simply another choice of generalized speeds corresponding to u / . Thus, Equations (5.16- 
5.19) prove the generalized Jacobian relationships of Equations (5.17— 5.7). 

Using the generalized Jacobian notation, the relationships between the two sets of generalized speeds 
and their derivatives can be given by the following equations: 

«' = ^(q)u + Ji(q) 

« = (u'~ J" t (q)) (5.20) 

« = ^ _1 (q)(u'- j'(q,u)J' _1 (q)u'- ,j(q,u).J _1 (q),7 t (q) - j’ttq.u)) 

The equations in 5.20 justify the calculations in Equation (5.12) and Equation (5.13) to derive desired 
generalized speeds and their time-derivatives from desired task-s pace quantities. 

5.2.2 Partial Velocities 

Partial angular velocities and partial velocities play large roles in the development of Kane’s dynamical 
equations for generating the equations of motion of a system. This section examines the relationship 
between two sets of partial angular velocities and partial velocities that result from any two choices of 
generalized speeds. 

’in a holonomic system, the number of generalized speeds equal the number of generalized coordinates, such that W(q) 
and W'(q) are both square and invertible. 



5.2. Generalized Relationships 


55 


The angular velocity in reference frame N of a rigid body B and the velocity in N of a point P on 
B can be expressed uniquely as [15]: 

n 

u, = 5> r u r + u, t (5.21 ) 

r= 1 

and 

n 

v = ^v r u r + v t (5.22) 

r=l 

where u? r , v r , u?*, and v* are functions 10 of q and time t> and u r is the rth generalized speed. The 
vector, u; r , is the rth partial angular velocity of B in N , and v r is the rth partial velocity 11 of P in N. 
Again, when there is no prescribed motion, and v* are zero. 

Rewriting Equation (5.21) and Equation (5.22) in matrix notation gives: 


UJ = [w r ] U + U>t 


(5.23) 


and 

V = [v r ] u + v t (5.24) 

where the rth column of the matrix [u> r ] contains the rth partial angular velocity, and the rth column 
of the matrix [v r ] contains the rth partial velocity 12 . 

Using the two sets of definitions for generalized speeds in Equation (5.14) and Equation (5.15), the 
expressions for ur and v can be expressed as: 

U> — [u?r] U + u>t = [u^l u' + (5.25) 

(5.23) L rj t 


and 


y=[v r ]u + v t = [v' r ]u' + v' t (5.26) 

( 5 . 24 ) 

10 Because of the dynamics of physical systems, these partial- velocity functions are independent of the generalized speeds. 

11 Note that the partial angular velocities art the partial derivatives of u> with respect to the generalized speeds, and the 
partial velocities are the partial derivatives of v with respect to the generalized speeds. That is: 


w r 


^ gw 
du r 


V r 


£ fry 
5u r 


In practice, w r and v r can be derived by inspection, since they are simply the coefficients of u r in the expression for u> and v, 
respectively. 

I2 The brackets notation represents the matrix comprised of all partial velocity vectors, respectively: 

[U> r ] = [ Wj ... Uf n ] [v r ] = [ Vi ... V„ ] 



56 


Chapter 5. The Task-Space Concept 


where [u> f r ] and [v' ] are the partial angular velocities and partial velocity matrices, respectively, associ- 
ated with u'. 

Utilizing the relationship between the generalized speeds, substitute Equation (5.16) for u' into 
Equation (5.25) and Equation (5.26) to give: 

[w r ] u + u?t = [v'r ] W / (q)W" 1 (q)u 

+ H] (w'(q) - W # (q)W- 1 (q)W ( (q)) + wj (5.27) 

and 

[v r ] u + v t = [v' r ] W'(q)W“ 1 (q)u 

+ K] (w'(q) - W'(q)W-'(q)W t (q)) (5.28) 

Equating the coefficients of u results in the relationship between the two sets of partial velocities: 

M = K] W, (q)W _1 (q) ( 5 =^ Kr] «7(q) (5.29) 

[Vr] = [v:]W'(q)W-'(q) ( ^K]j(q) (5.30) 

These relationships will be used to relate the sets of generalized active forces associated with the two sets 
of generalized speeds. 

5.2.3 Generalized Active Forces 

This section develops the relationship between any two sets of generalized active forces. The generalized 
active force acting on a rigid body B is defined in Kane [15] according to the following: 

If B is a rigid body belonging to a holonomic system S possessing n degrees of freedom in a 
reference frame N, and a set of contact and/or distance forces acting on B is equivalent to a couple of 
torque T# together with a force R q whose line of action passes through a point Q of B , then (F r ) q, 
the contribution of this set of forces to the generalized active force F r for 5 in TV is given by 

(Ft) b = • Tb + v? • Rq (r=l,...,n) (5.31) 

where uf and v{? are, respectively, the rth partial angular velocity of B in N and the rth partial 
velocity of Q in N. 
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Note that the dot-products are equivalent to the following: 

^ (5.32, 

v?-Rq = v^TRq 

Sucking the contribution of 5 to the n generalized active forces into a column vector re-expresses 
Equation (5.31) in matrix notation as 13 : 


[(F r ) B ) 



«f -t B 


vf Rq 

(5.31) 

-T B . 

+ 

,v£ R 


" : t Tb ’ 


’ vfrct^ ' 

(532) 

. W n T Tb . 

+ 

. V n T R<? . 

(5.253.26) 

;u,f] T T B + [v?] T R« 


(3.33) 


where the [( F r ) g] is the set of generalized active forces contributed by forces acting on body B. 
Summing over all bodies in system 5 yields the generalized active force vector for the system: 

'A E K] T Tb + [v?] T R, (5.34) 


all 

bodies 


Using the alternate set of generalized speeds, u', the generalized forces given by Equation (5.34) 
becomes: 

£ p r B ] T T fl +[v r »2] r R<, (5.35) 


F' 

(5.34.5.25. 5.26) 


all 

bodies 


where the primed notation indicates the generalized active forces and partial velocities associated with 


u' 


To derive the relationship between F and F', substitute the partial velocity relationships of Equa- 
tion (5.29) and Equation (5.30) into Equation (5.34): 

i T 


(5.34,5.29, 5.30) 


£ ([u,' r B ]W'(,)W-‘(q)) r T B +([v?2]w'(q)W-'(q)) Rq 


all 

bodies 


13 The matrix notation is defined as: 


[(Ft)b\ = 


(F,) b 

(B j n)s 
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£ (W , (q)W -1 (q)) r [u>' t B ] T T B + (W f (q)W~\q)) T [v^R* 

all 

bodies 

(w / (q)W- 1 (q)) T £ L^Tb+I^Rq (5.36) 

all 

bodies 

Using Equation (5.35) as the expression for generalized forces results in the relationship: 

F = fw'(q)W- 1 (q)) r F' (5.37) 

(5.36,5.35) V / 

Hence, taking the definition of generalized Jacobian in Equation (5.17) gives: 


F = 

(5.37,5.17) 


•7(qfF' 


(5-38) 


Equation (5.38) is a general relationship between two sets of generalized active forces for any two sets 
of generalized speeds. 


Utilizing the following definitions for generalized speeds of a single-manipulator robot: 

u = q 

u' = x tip 

the corresponding generalized active forces are: 

F = r 

F' _ p tip 

Substituting these into Equation (5.38) and utilizing the endpoint Jacobian gives: 

r = J r (q)F t<p (5.39) 

This is the familiar relationship relating actuator torques to endpoint forces. The development in this 
section provides a more formalized proof of this relationship than the usual proof involving statics and 
virtual work 14 . 


5.2.4 Equations of Motion 

The equations of motions for a system of rigid bodies, such as a rigid-link robot, in terms of a set of 
generalized speeds, u, as: 

F = M(q)u + C(q, u)u + G(q) + ft 


u See Appendix A.4 for the virtual work proof. 


(5.40) 
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where the vector, f*, has been added to include possible forcing functions that keep the system in some 
prescribed motion. Using another set of generalized speeds, u', the equations of motion are equivalently 
expressed as: 

F' = M'(q)u' + C'(q, u')u' + G(q) + i[ (5.41) 

This section demonstrates the relationship between the terms in the two sets of equations of motion. It 
also demonstrates the motivation behind the choice for the task - space adaptive control law. 

Using the generalized Jacobian relationships defined in Equation (5.20) to substitute for u and u 
in the first set of equations of motion gives: 

F = M(q)J' _1 (q)u / + (C(q,u) + M(q) j"(q, u)) J~ x u' + G(q) 

+f t - M(q) (j'(q,u)J' _1 (q)J't(q) + j’^q.u)) 

-c(q,u)j" 1 (q)^'t(q) (5-42) 

Prcmultiplying Equation (5.42) by and using Equation (5.38) to substitute F' for F gives: 

F' = J" _T (q)M(q)J' _1 (q)u' 

+J~ T ( q) (C(q,u) + M(q)J r - 1 (q) j'(q.u)) J~\ q)u' + ^- T (q)G(q) 
+J~ T { q) (ft - M(q)J- 1 (q) (j r (q,u)J- 1 (q)J't(q) + *7t(q,u)) 

- C(q, u) J~ X (q) J t(q)) (5-43) 

When there are no prescribed motions, f t and J7't(q) are zero, thus. Equation (5.43) simplifies to: 

F' = 

+J~ T ( q) (C(q, u) + M(q)^7’ -1 (q)JT(q, u)) J~ l { q)u' 
+J’ _T (q)G(q) (5-44) 

Hence, without prescribed motions, there is a simple transformation between the terms of two sets 
of equations of motion. Comparing terms in Equation (5.44) with the equations of motion in 
Equation (5.41) developed for u' gives: 

M'(q) = J’- T (q)M(q)J'- , (q) (5.45) 

C'(q, u') = J~ T ( q) (c(q, u) + M(q)J"* 1 (q) ^(q)) J ~ 1 (q) (5.46) 

G'(q) = ^- r (q)G(q) (5.47) 
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Similarly, one can show that: 

Y'(q,u',u',u') = J" r (q)Y(q,u,u,u) (5.48) 

where Equations (5.20) are used to express u and u in terms of u' and u'. 

Equation (5.43) and Equation (5.44) demonstrates the relationship between the equations of motion 
developed with any two sets of generalized speeds. When there are no prescribed motions, as is usual 
in most robotic applications, there exist direct one-to-one mappings between the two sets of inertia 
matrices, nonlinear matrices, and gravity torque vectors, given by Equations (5.45-5.47). 


5.2.5 Task-Space Control Law Revisited 

Employing the generalized relationships developed in this section, the fctr£-space adaptive controller 
given in Section 5.1 can be written in a simple form 15 : 

(5.49) 

(5.50) 

where, 


= M* a5fc (q)yjT* + C ta ‘*(q, y l r k )yT k + G ta ’ fc (q) + f* 


+ (Kvj,y ' + Kpj,y‘ 


= W(q)Wq)P* 



M'(q) = ^- T (q)M(q)^- , (q) 

C'(q,u') = J'- T (q)(C(q ) u) + M(q)^- , (q)^(q)) ^"‘(q) 
G'(q) = ^- T (q)G(q) 


Hie control law can be expressed in terms of the parameter vector as: 


F tagk ( = Y ta9k (q, yf k , yjj”* y^fi + f l t agk + \Kv v y + K/^y* 


The corresponding adaptive update law is: 


n rt-Krtask ^ / 


± ta8k . ^rtaak 


(5.51) 


(5.52) 


,5 Just prcmultiply Equation (5.8) by J T (q). 
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Comparing Equations 5.49 and 5.52 with the original joint-space control law, 

r = M(q)qj + C(q, q<*)q<i + G(q) + Kvq + Kpq (5-53) 

and the parameter adaptation law, 

d = rY r (q, q d , q d , q d ) (q + cq) (5.54) 

shows that the task-s pace adaptive controller is completely analogous to the original control law, except 
for the additional term, f£ asfc , to handle prescribed motions. Equations (5.49) through (5.52) show 
very effectively that control is performed completely in task space. This provides a satisfying justification 
for the form of the taskspzct adaptive controller, at least on an intuitive level 16 . 

Although the form of the adaptive controller given in Equations (5.49-5.52) appears much simpler 
than that offered in Section 5. 1 , it is, in general, actually much more difficult to implement. Depending 
on the choice of the taskspzcc objectives, the terms in the equations of motion developed in task 
space, M toflfc (q), C task (q> y ta9k )> and G <a * fc (q), can be much more complex than those derived for 
another set of generalized speeds. Thus, the tasks pace adaptive controller given in Section 5. 1 provides 
maximum flexibility for implementation. One can derive the system model utilizing a set of generalized 
speeds that minimizes the complexity, while still able to control the system in any other choice of 
taskspzcje objectives 17 . 

The following sections provide examples of valid tasks pace control objectives other than simple 
endpoint control. 


5.3 Control in a Noninertial Reference Frame 

Controlling in a noninertial, or accelerating, reference frame is an important control mode for a free- 
flying space robot 18 . This mode is useful when manipulating in the frame of the robot — for example, 
when performing docking or assembly while sensing both mating parts from the mobile robot base. The 

16 As Chapter 4 shows, however, the stability proof did not not carry through to task-space quite as readily: An additional 
condition that the manipulators stay away from singularity was required. 

l7 This is also why this dissertation continues to distinguish between task-space vectors and generalized speeds, despite 
showing that the derivative of y* a,fc is simply another set of generalized speeds. 

18 Please note that controlling in a moving or noninertial reference frame is a separate issue from simply controlling from a 
moving base. One can perform control from a moving base in a inertial reference frame as well as from an noninertial one. 
The difference is whether the desired trajectories are specified as fixed in an inertial or noninertial frame. 
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robot base may be rotating or otherwise accelerating, making the robot frame a noninertial reference 
frame. 

Although all development up to this point has assumed an inertial reference frame, this section 
shows that control in a noninertial reference frame can still be expressed as a task - space objective, making 
direct application of the task-space adaptive control law feasible. This section also presents an example, 
based on the familiar two-link arm, that demonstrates the differences between inertial and noninertial 
frame control: it shows that for a space robot, the noninertial-reference-frame controller can be more 
fuel efficient than the inertial-frame controller. 

5.3.1 Adaptive Endpoint Control in Noninertial Frame 

The adaptive endpoint control in a noninertial frame also utilizes a generalized Jacobian — but one 
derived in the noninertial frame. The kinematic relationship, from Equation (4.15), is: 


x = k(q) (5.55) 

which is independent of reference frames 19 . Differentiating this relationship is dependent on the 
reference frame in which the derivative is taken. Doing so in a noninertial frame, B> and expressing it 
in terms of generalized speeds gives: 

B x = B J'(q)u (5.56) 

(4.23) V ' 

where 5 J'(q) is the endpoint Jacobian with respect to the B reference frame. Following the same 

development as the original Jacobian yields the following inverse relationships: 

U = B J' -I (q) B x (5.57) 

u = B J' -, (q)( B x- B j'(q,u) B J /-1 (q) B x) (5.58) 

Thus, choosing the generalized Jacobian to be: 

J(<l) = B J'(q) (5.59) 

allows direct application of the task-spact control law in Section 5.1. 


Note that although the noninertial Jacobian does not contain any information about the motions of 

the noninertial reference frame, the centrifugal and Coriolis effects of the the accelerating reference frame 

,9 The physical position vector to the endpoint may be expressed in terms of different coordinate systems; but, as a physical 
vector, it is of course independent of the r ef eren ce frame . 
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arc still being compensated by the C'(q, u<*)ud and ft terms in the control law (see Equation (5.8)): 
Those terms do include the motions of the reference frame. 


Example 

The modified two-link arm example illustrated in Figure 5.2 is used to compare the difference between 
inertial- and noninertial-ffame control. The familiar two-link arm is placed on a mobile base with 
a single rotational degree of freedom, represented by < 73 . The center of both coordinate systems is 



Figure 5.2: Two-Link Arm on a Turntable 

qi and qj represent the shoulder and elbow joint angles, and <73 is the rotation angle of 
the turntable. x tip is the (invariant physical) position vector from the shoulder to the the 
manipulator endpoint. a\ and a 2 arc mutually orthogonal unit vectors fixed in the inertial 
frame , while bi and b 2 arc mutually orthogonal unit vectors fixed in the turntable frame. 


assumed to be at the shoulder. The inertial reference frame is defined by the mutually orthogonal unit 
vectors, ai and a 2 > the rotating reference frame of the turntable base is defined by the unit vectors, b\ 
and b 2 . 
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Define the generalized speeds as: 


u\ = q\ 
u 2 = q i + q 2 


(5.60) 


«3 = 93 

Also define for inertial control as a combination of the endpoint position — expressed in the 

inertial coordinate system — and the base rotation: 


task 

•7 


a, 


x Up ■ a 2 


q 3 


(5.61) 


y task ^ 


For noninertial frame control choose a combination of the same endpoint position — but expressed in 
the noninertial coordinate system — and base rotation: 

x tip ■ bj 

x tip • b 2 | (5.62) 

q 3 

The inertial reference frame Jacobian can be expressed as: 

-li sin(g! + q 3 ) -l 2 sin(gi +q 2 + q 3 ) -l x sin(g, + q 3 ) - l 2 sin(g] + q 2 + q 3 ) 
J, (q) = h cos(gi + q 3 ) l 2 cos (gj + q 2 + q 3 ) l\ cos(gi + g 3 ) + 1 2 cosfai + g 2 + g 3 ) 

0 0 1 

(5.63) 

and the base-relative Jacobian is: 

r 

—h sin(?i ) -l 2 sin(g 2 + q 3 ) 0 

J, (q) = I iicos(gi) l 2 cos(q 2 + q 3 ) 0 (5.64) 

0 0 1 

Both controllers compensate for inertial effects, since each set of desired task-space motions is resolved 
first into equivalent desired generalized speeds and generalized-speed derivatives before incorporating 
them into the dynamics equations (see Equation (5.8)). The main difference appears in the feedback 
portion of the controllers. The last term in the control law of Equation (5.8), 


B V 


(q) (Kv y y ' 


+ Kp„y : 


task 


) 


(5.65) 
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illustrates the difference between the controllers. 


Consider the effect of a pure endpoint error given by: 

x = Axbj 

= Axcos(g3)ai + Axsin(g3)a2 


( 5 . 66 ) 

(5.67) 


Also consider, without loss of generality, that the position gains are all unity. 

The feedback law of the inertial frame controller asks for a compensating generalized force given by 
(see Equation (5.8): 


J'(q) 


T~ 

X = 


( 5 . 63 , 5 . 67 ) 


-h sin(gi) 

— h sin(gi + <fe) 

-l\ sin(gi) - l 2 sin(gi + q 2 ) 


Ax 


( 5 . 68 ) 


The compensating generalized force called for by the base-relative controller is (see Equation (5.8)): 


B 


J'(q) 


T~ 

X = 


( 5 . 64 , 5 . 66 ) 


—l\ sin(gi) 
-l 2 sin(g t + q 2 ) 
0 


Ax 


(5.69) 


The first two terms in the generalized-force (mathematical) vector represent output from the 
shoulder and elbow motors. Equation (5.68) and Equation (5.69) show that both controllers will 
request the same amount of arm motor torques to compensate for the error. The difference is in the last 
term of the generalized-force vector, which represents the torque on the mobile base. Equation (5.68) 
shows that using the inertial reference frame Jacobian, the controller will compensate in the turntable 
for the effects of applying torques to the two-link arm. Equation (5.69) shows that the controller using 
the noninertial Jacobian ignores this effect. The base motor is actuated only in response to errors in the 
base orientation or angular rate, but not to endpoint-error feedback. 

Although one can argue whether the latter is the desired behavior one would expect from a 
noninertial frame controller, this behavior is actually beneficial for space-robot control. The controller 
can place relatively high feedback gains on the base-relative position and velocity of the end effectors 
or a manipulated payload to cancel small tracking errors without incurring thruster firing^ , saving 


20 The thrusters will fire in response to base position and orientation errors. 
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precious fuel. The inertial-frame controller will always fire the thrusters 21 to compensate as needed for 
the feedback torques applied to the arms. 


5.4 Space-Robot Control — Single Manipulator 


The control of a space robot involves controlling the robot base and the manipulator simultaneously 
to achieve some desired trajectory of the payload, end effector or arm joints. For payload control, a 
natural task - space control mathematical vector is simply the one containing both the payload and robot 
base positions and orientations. Additionally, the payload motions can be expressed in either an inertial 
reference frame or the base-relative frame. 

Figure 5.3 shows an example utilizing a planar, one-arm space robot. The base has three degrees of 
freedom — two in translation and one in rotation — and the arm has two degrees of freedom, totalling 
five for the system, as represented by q\ through q$. 

For endpoint control in the inertial reference frame choose 22 : 


y 


task 


x°«. ai 
x OQ • a.2 
9 3 


94 

95 


(5.70) 


such that, 


• task 


^x^ • ai 

A x9 • &2 
93 


94 

95 


(5.71) 


Implementation of task-space adaptive controller, therefore, will provide adaptation while simultane- 


ously controlling the manipulator endpoint position and the robot base position and orientation. 


21 Actually, the thrusters will not lire unless the compensating forces are above the threshold of the on/off thruster controller. 
This, however, prevents the use of higher gains for the control of endpoint or payload positions. 

22 For the special case that the end effector grasps the object at its center of mass, endpoint control is the same as object 
control. 
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q\ and qi represent the shoulder and elbow joint angles . q$ and q\ represent the position of 
point B on the robot base, resolved into components along aj and a2, respectively. q$ is the 
orientation of the robot base, x 0 ® is the position vector from the center of the coordinate 
system fixed in A to the manipulator endpoint , while is the position vector from the 
center of the robot base to the endpoint . a\ and *2 are mutual// orthogonal unit vectors fixed 
in the inertial frame, while bi and b2 are mutually orthogonal unit vectors fixed in (painted 
on) the robot base frame. 


On the other hand, for endpoint control in a useful combination of frames, we can choose the 
task-space vector to be: 


y 


task 


• bi 

x BC ? • b2 

93 


94 


(5.72) 


95 
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such that, 


^ task __ 


B x9 • b 2 

93 

94 

95 


(573) 


which generates a controller that simultaneously controls the endpoint in the base frame and the robot 
base in the inertial frame . A significant benefit of the base-relative controller, as mentioned in Section 5.3, 
is that higher feedback gains can be used on the endpoint tracking errors without incurring excessive 
thruster firings, thereby saving fuel. 


5.5 Control with Motion Constraints 


Motion constraints occur when the manipulator comes into contact with a surface or another object. 
These constraints can often be incorporated into the task-space vector by considering the relati ve velocity 
between the endpoint and the surface or payload object. Figure 5.4 shows a planar arm about to grasp 
an object. The payload is modelled as part of the system, where its position is described by q$ and < 74 . 
The object is assumed, for simplicity of presentation, to be circularly symmetric, and the grasp point, 
x°®' , is at the center of the object 23 . 

For this situation it is advantageous to let the task - space vector be composed of the endpoint position 
and the relative position between the endpoint and the grasp point on the object: 


ytask _ 


x°® • ai 


x 0< 2 • a2 


x OQ_ x OQ') 

•ai 

x OQ_ x OQ’\ 

•ai 


( 5 . 74 ) 


^The only reason for this requirement is because, with only two manipulator joints, and no orientation control at the end 
effector, the orientation of the object is uncontrollable. The problem can be expanded readily to include a rotational joint at 
the gripper. 
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q\ and <72 represent the shoulder and elbow joint angles. The object is modelled as part of the 
system , and its position is given by <73 and <74. Q marks the endpoint of the manipulator, and 
Q f is the grasp point on the object . When the manipulator grasps the object, Q coincides with 
Q\ a\ and az *re mutually orthogonal unit vectors fixed in the inertia/ frame. 


such that 


x^ * ai 
x^ • a.2 

(*9 - x9' ) • ai 

(x^ - x^) • ai 
J ( q ) 0 
J(q) -hx 2 
= J{ q)q 


(5.75) 


(5.76) 

(5.77) 


where J(q) is the endpoint Jacobian, I 2x 2 is the 2 x 2 identity matrix, u = q, and «J(q) is the 
resulting generalized Jacobian. To enforce the motion constraints, the controller simply specifies that 
the last two terms of the desired trajectories, y^ ek , and y l f sk , be zero. 

This example will be useful when formulating multiple-manipulator cooperative control in Chap- 
ter 6. The force constraints that occur at the grasp point will also be examined in Chapter 6. 
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5.6 System-Momentum Control 

Control of the total system momentum can be useful for space robots as a fuel savings measure. By 
controlling endpoint or object motions while maintaining constant system momentum, no thrusters 
will be fired, ensuring that no fuel is expended. With no thrusters, the robot base will of course react 
to the motions of the manipulators. The controller compensates appropriately for these base motions 
to allow accurate trajectory tracking of the endpoint or payload object. 

This section shows that the system linear momentum and angular momentum can be expressed 
as yet another set of tar£-space objectives. In particular, they are shown to be a linear function of 
generalized speeds 24 . 

The linear momentum L 5 of a system of p bodies is the sum of the linear momentum of each body 
i in the system 25 : 


p 


L 5 = E L ‘ 

1=1 

(5.78) 

= £,my 
1=1 

(5.79) 

where v** is the velocity of the center of mass of body i. 


Expressed in terms of generalized speeds, L 5 becomes 26 : 


P n 


L s = 

t=l r=l 

p n 

(5.80) 

i=l r=l 
n V 

(5.81) 

= U r 

r- 1 i=l 

(5.82) 

= 

r=l 

(5.83) 

II 

"F 

S3 

(5.84) 

where hf is the partial linear momentum given by: 


L ? = 

»=1 

(5.85) 


24 See Koningstein [19] for more details. 

25 Formally, this is the linear momentum of S in frame A if the velocities are also in A. 

26 Because the whole system is being modelled, there are no prescribed motions, making vf zero. 
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and the matrix notation, j , is given by: 

M = [l? ... L»] (5-86) 

Similar arguments follow for the central angular momentum of a system of p bodies about 

the system mass center S *: 


H 5 ' 5 ' = 

i= 1 

= ^ I * + x 5t * x mjv f * 

i=l 

= ]T ]T + X 5 *** x 771* ^2 y r U r 

t=l r-\ r = 1 

r=l \t=l / 

= £ (tm-''- + **•'• xl;) «, 

r= 1 \t=l / 

= EHf'u r 
= [Hf]u 


(5.87) 

( 5 . 88 ) 

(5.89) 

(5.90) 

(5.91) 

(5.92) 

(5.93) 


where x is the position vector from the system center of mass to the center of mass of body i, and 
is the partial central angular momentum of the system given by: 


Hf'=tHf+x sv xL‘ 

•=1 


(5.94) 


The matrix notation, 5 J , is given by: 

[h?^] = [ H? /s ‘ ... H? s ’’ 


(5.95) 


Thus both L 5 and H 5 / 5 * can be viewed as alternate generalized speeds (see Equation (5.15)). As 
such, they are valid choices for elements in the task - space vector 27 . 

When no adaptation is required, the fcw£-space controller presented in Section 5.1 can be used 
direedy to control system momentum. That controller, however, cannot in theory be implemented for 

27 Since the task - space vector needs to have n elements — matching the degrees of freedom in the system the system linear 
momentum and angular momentum by themselves do not of course provide sufficient degrees of freedom to complete a 
ftzr/fr-space mathematical vector. 
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adaptation, because the terms in the generalized Jacobian corresponding to j and j contain 

unknown parameters, such as masses, moments of inertia, and the center-of-mass location for each body 
t. In this case, the adaptive controller formulated completely in task space, as given in Section 5.2.5, 
may be required. Utilizing this form of the controller, however, increases controller complexity. The 
noninertial reference frame controller may be a better candidate for fuel-efficient control. 

The one-arm space robot illustrates the control of system momentum. Figure 5.5 depicts the system. 
Since this is a planar system, the system linear momentum involves two degrees of freedom in the plane, 



Figure 5.5: One-Arm Space Robot — Momentum Control 

qi and q^ represent the shoulder and elbow joint angles. q$, q+, and q<> represent the position 
and orientation of the robot base. Q marks the manipulator endpoint , and S* marks the 
system mass center at this insranr. ai and aj are mutually orthogonal unit vectors fixed in the 
inenial frame , while bj and b 2 are mutually orthogonal unit vectors fixed in the robot base 
frame. a$ and b 3 are unit vectors perpendicular to the plane and point out from the plane. 


and the angular momentum only involves one degree of freedom — rotation about an axis in a direction 
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' task 


perpendicular to the plane. Choose the task-s pace mathematical vector such that its derivative is: 

• ai 

• a 2 

L 5 a, 

L 5 a 2 

H s/5 ’ a 3 

J(q) 

M 

[ [ h H J 


(5.96) 


(5.97) 


(5.98) 


where J(q) is the 2x5 endpoint Jacobian in the inertial frame, is the 2x5 partial linear 

momentum matrix, and j is the 1x5 partial angular momentum matrix. 

To maintain zero system momentum while controlling the endpoint, simply set the last three terms 
in the desired toskspace trajectories, yf $k , y l d ask , and yf ek , to zero. 

Base-relative endpoint control can be achieved by choosing to use the base-relative endpoint Jaco- 
bian, B J(q),iny loafc28 . 


5.7 Redundancy Management 

Ziflfr-space control offers a limited solution to the control of redundant mechanisms. Whether a system 
is redundant involves consideration of the number of degrees of freedom (DOF) in the system versus 
the number of degrees of freedom to be controlled. If the number of controlled degrees of freedom is 
less than that of the total system, the control is said to be redundant. 

The classic example is the control of a manipulator end effector in six degrees of freedom — three in 
position and three in orientation — with a 7 DOF manipulator. The endpoint Jacobian for this system is 
nonsquare, so the direct matrix inverse used to derive joint rates from endpoint velocities is unavailable. 
Most redundant-control research has concentrated on utilizing the generalized inverse of the Jacobian: 

<U = 3 T (q) (j(q)J T (q))"’ U + (l„*„ - J r (q) (j(q)J T (q))"' J(q)) (5.99) 

M In actual implementation, proportional or pulse- width- modulation thruster control would still use a significant amount 
of fuel, because the base-relative endpoint errors, while small, do not stay exactly zero. Combining this control mode with 
on-off thruster control, on the other hand, would keep fuel use zero for extended periods. 
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where x t can be any vector. The last term represents motion in the null space of J(q); that is, internal 
motion that will not affect the endpoint motion. Without x*, the desired set of joint rates q<* minimizes 
the energy expended in meeting the desired endpoint trajectory x<j. Many researchers argue of course 
for the attractive option that adding x* can make the manipulator achieve additional objectives, such as 
singularity avoidance or obstacle avoidance, while tracking endpoint trajectories. In practice, however, 
it often occurs that choosing x* to achieve the additional objectives is difficult. Moreover, solving the 
generalized Jacobian inverse, J T (q) (j(q)J T (q)) , can be computationally expensive. 

The task-space control approach, however, allows direct choice of the additional objectives. In the 
one-arm space robot example, the additional objectives are represented by either the direct control of 
the robot base motions or the system momentum 29 . For those tasks involving redundant manipulators 
where additional control objects can be appropriately chosen, task-s pace control offers a simple solution. 

5.8 Conclusions 

Task-space control is a powerful concept for use in the control of robotic systems. It allows the 
control engineer to develop the system model, or equations of motion, in its simplest form utilizing an 
appropriate set of generalized speeds; yet it allows him to control any variables that can be expressed 
as functions of the generalized coordinates. The task-space control vector can contain a mixture of 
physically dissimilar quantities, containing joint motions, Cartesian motions, motion constraints, and 
even system momenta, as long as the size of the tar£-space vector matches the total number of degrees of 
freedom of the robotic system. Finally, utilizing the stability proof for the endpoint adaptive controller, 
one can show with negligible effort that the same proof demonstrates the stability of the tosk-s pace 
adaptive controller. 


29 This is also the approach chosen by Seraji [35] for the control of single-manipulator robots. 
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Multiple manipulators grasping a common object form closed kinematic chains. Given the fully reduced 
set of equations of motion for such a system, the new tar£-space controller may be used direcdy for 
control. The closed-kinematic-chain equations of motion, however, can be very complex to derive, 
hampering implementation. System concatenation is a concept that eliminates the requirement for 
deriving the complex closed-chain equations, but can still work well with the new task-s pace adaptive 
controller to provide cooperative control for multiple manipulators. 

System concatenation is a modelling technique that provides for efficient, incremental generation of 
total system models for multiple, interacting system, such as multiple arms cooperatively manipulating 
an object. It takes full advantage of models already developed for each manipulator or robot subsystem 
to minimize the effort in deriving the total system models. This formulation eliminates the need to 
develop and solve for the closed-kinematic-chain equations of motion, simplifying implementation. It 
also keeps separated the parameters of each robot subsystem, allowing the ability to separately tune 
the adaptive control for each subsystem. 

Essentially, system concatenation simply stacks, or concatenates, the equations of motion of each 
subsystem into a larger, total system model. The coupling between each subsystem is handled through 
constraints — motion constraints and force constraints. This idea is not new. Many simulation tech- 
niques are based on this concept. Its use in controls in its generalized form, however, is to the authors 
knowledge, novel. System concatenation does of course increase the order of the set of system equations. 
This disadvantage, however, is offset by the ease of forming the equations of motion, minimizing the 
possibilities for error when the algorithm is implemented on a digital computer. 
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The system concatenation approach utilizes task-spacie control to incorporate the motion constraints 
into the task-space control objective, and specifies desired task-space trajectories that are consistent with 
these constraints. Explicit modelling of the force constraints also allows the controller to specify load 
distribution amongst the manipulators in addition to specifying internal forces to be applied on the 
manipulated payload. 

Section 6. 1 defines the structure of the equations of motion for the concatenated system. Sections 6.2 
and 6.3 derive the motion and force constraints in terms of the generalized Jacobian, allowing them 
to be incorporated into the task-space controller. Section 6.4 utilizes the constraint forces to derive 
the mapping from object forces and torques to end-effector forces and torques — a mapping that allows 
the specification of load distribution and of the “squeeze” forces experienced by the object being 
manipulated. 

6.1 Modelling 

A pair of manipulators grasping a common object form a closed kinematic chain. The motion 
constraints at the grasp points reduce the number of degrees of freedom for the system. The equations 
of motion for the reduced-order system involve solving the closed-kinematic equations — a very complex 
set of equations in general. Moreover, to perform adaptive control, these equations must be solved 
symbolically in order to determine how the equations of motion depend on the physical parameters of 
the system. While this can be done for any system, updating the model as the system changes — adding 
manipulators, for example — can be extremely costly. 

As Koningstein [19] demonstrated at ARL, however, a well-designed controller did not require the 
reduced order equations of motion. The controller treated the system as an open-chain system by 
breaking the kinematic chain at a grasp point and modelling the payload as an additional link on one 
of the manipulators. The controller continued to enforce the motion and force constraints at the grasp 
point by specifying desired trajectories and desired forces that were consistent with the constraints. 
Doing so allowed the manipulator system to perform its task without putting undue stress on the 
manipulated object. 

System concatenation carries this approach a further step. It breaks the kinematic chains at all grasp 
points 1 . One benefit is that the controller can take advantage of the system models already developed 


1 If there are more than two manipulators there are more than one kinematic chain. 
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for individual manipulators and objects. It does not have to modify the equations of motion of a 
manipulator to include the object as an additional link. All manipulators are treated equivalently. Most 
importandy, this approach is applicable for any number of manipulators and objects. 

Another benefit of system concatenation for adaptive control is that the physical parameters of 
individual manipulators and objects are kept separated. This allows the adaptive controller to isolate the 
physical parameters associated with each subsystem into separate portions of the adaptable parameter 
vector. If only a subsystem, such as the payload object, is unknown, the parameter separation property 
minimizes the number of parameters that must be adapted. The parameter separation property also 
ensures that the controller can take full advantage of all that are known in the system. At the same 
time, separable parameters does not prevent the adaptive controller from simultaneously adaptive to all 
the parameters for the entire system. 


6.1.1 System Concatenation Formulation 

System concatenation considers individual robots and objects as subsystems of the entire manipulation 
system. Let each of these subsystems i be modelled as: 


F i = M' t (q t )u* + C' t (q„Ui)u i + G'i(qi) = Y'^q*, u i? u t) xu)0 t (6.1) 

where q* E R ni is a vector of generalized coordinates 2 , Ui 6 R ni is a vector of generalized speeds, F* 6 
R n * is the vector of generalized active forces, M'i(qi) E R n ' xni is the symmetric, positive-definite 
inertia matrix, C\(qi, Ui) E R niXn * is the matrix of Coriolis and centrifugal terms, G'i(qi) E R n * 
is the vector of gravity torques, Y / i(qi, Ui, U{, Ui) E R n » XPt is the regressor 3 , and E R Pl is the 
parameter vector, all defined for the subsystem i. The equations of motion for an entire system with m 
subsystems can be expressed as: 
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( 6 . 2 ) 


2 Generalized coordinates are used in the development for maintaining mathematical generality to facilitate implementation 
on a digital computer. In all the examples, the generalized coordinates are simply chosen to be convenient geometric coordinates 
that can be easily measured. 

3 The term “regressor" is typically used in system identification to refer to the mathematical matrix that contains the states 
of the system and none of the plant parameters. 
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0 



+ 



XC 


(6.3) 

(6.4) 


where xc € R^ represents N motion constraints, € R n is vector of generalized constraint forces 
that result from the motion constraints, and n = ni + . . . + n m is the degrees of freedom for the entire 
system. The equations of motion can be expressed in terms of the regressor and parameter vectors as: 
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(6.5) 

( 6 . 6 ) 


Note how the individual parameter sets, Oi , are kept separated. 

This demonstrates that the equations of motion for the entire system is almost trivial to form, given 
the individual equations of motion. What remains are the examinations of the motion constraints and 
the generalized constraint forces. 


6. 1.2 Example 

The example in Figure 6.1 illustrates the system concatenation approach 4 . The picture at the left 
represents the separate subsystems and the one at the right represents the entire system when connected. 
The individual equations of motion, ignoring gravity, are listed with each subsystem. When not 
grasping the payload, the system has seven (7) degrees of freedom. When grasping, the constraints 
reduce the system to three (3) degrees of freedom, most easily seen as the position and orientation of the 
object. That is, given the position and orientation of the object, all the arm joint angles are uniquely 
determined. 

The generalized coordinates, q a rmi € R 2 and q arm2 € R 2 , correspond with the joint angles 
for the two manipulators, and q € R 3 represents the position of P — the center of mass of the 

4 The example shows that system concatenation simply utilizes D’Alemberts Principle to combine the equations already 
developed for the individual systems to form the foil system equations of motion. One could have started with D’Alemberts 
Principle, of course, to derive the foil equations of motion from scratch. 
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The system of two manipulators and the not-yct-capturcd payload object is illustrated at the left 
as separate subsystems. Their individual equations of motion are listed below each subsystem. 
System concatenation brings cbe separate subsystems together to model the complete system 
after the object is grasped. i9 l and i9 2 are the endpoint velocities, i^ 1 and i Q * are the 
velocities of the grasp points, f^ 1 and f® 2 represent forces on each manipulator at its endpoint, 
and f^» and f®' 2 represent forces on the object at each grasp point. 


payload — and the orientation of payload body D : 


Qobj — 


p 

x. r • ai 

p 

• SL2 


a A 


Define the generalized speeds as the time derivatives of the generalized coordinates: 


Uflrmi = Qarmi 
U a rm 2 = Qarm.2 
^obj — Qobj 


( 6 . 7 ) 


( 6 . 8 ) 


Rather than generating the reduced set of equations of motion, system concatenation retains the full 
seven degrees of freedom and additionally models the constraints, resulting in the following equations 
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of motion: 
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and motion constraints: 

04x1 = 

where 04 X i represents a 4 X 1 vector of zeroes. 


- x«5 


(6.9) 


( 6 . 10 ) 


( 6 . 11 ) 


( 6 . 12 ) 


6.2 Motion Constraints 


The motion constraints in a system of multiple manipulators grasping an object are best represented by 
constraints on relative velocities — in both translation and orientation — between each manipulator end 
effector and its grasp point on the object. Specifically, the relative velocities are constrained to be zero. 
As Section 5.5 showed, these constraints can be adequately represented by task-space control objectives. 

Figure 6.2 illustrates the constraints for multiple manipulators. The relative velocity between each 
end effector Qi and its grasp point Q' x satisfies the relationship: 
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(6.16) 


where xc, € R 2 is the relative velocity between the two points, and € R 2x7 and [v^'j € R 2x7 
are the matrix of partial velocity vectors associated with Q, and Q[, respectively. 
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Figure 6.2: Motion Constraints 

A motion constraint exists at each grasp point. Qi is a endpoint of manipulator i, and Q\ is 
the associated grip port on the payload object . x Qt is the endpoint velocity ; and ± Q ' is the 
velocity of the grip port. When the object is grasped , the relative velocity between Qi and Q ' 
is constrained to be zero. The constraint velocity' xc t can be expressed in terms of a generalized 
Jacobian J Cy and the generalized speeds u. a \ , a 2 , and a 3 are mutually orthogonal unit vectors 
fixed in the inertial frame. 


Defining a generalized Jacobian component for the ith relative velocity xc< as 

J c '( q) = [v?*] - [v? ( ] 


(6.17) 


the relative velocity can thus be expressed as: 


xc ( = rr C ' (q)u (6.18) 

'(6.16,6.17) 

In general, there also may be rotational constraints at the end effector. If the manipulator has a 
solid grasp of the object, for example, then the relative angular velocity between the end effector endj 
and the payload D must be zero. These can be represented by: 
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= ([wr™* 7 ] - [^P]) u (6.22) 

= (q)u (6.23) 


The expression for angular velocity constraints is completely analogous to that of the linear velocity 
constraints of Equation (6.18). Hence, angular velocity constraints also can be incorporated into the 
task-space control vector. 

When the payload is grasped, these relative velocities are constrained to be zero. To enforce these 
constraints, the task-spa.ee controller incorporates these equations into the task - space control vector 
along with the payload object motions: 


X-obj 

XCi 


'task 


*Cn 


(6.24) 


L J 

The only requirement is that y ta9k has the same degrees of freedom as the open-chain system. 

By specifying zero for the constraint terms in the desired task-space vectors — y l f sk y y £***, and 
— the trajectory generator requests motions in the system that are completely consistent with the 
motion constraints. The controller simultaneously enforces these constraints and the desired payload 
motions. 


Example 


Using the same example in Figure 6.2 with a pair of planar arm and a rigid payload object, choose the 
task-space control velocity vector to be a combination of the payload Cartesian and angular velocities 
and the relative velocities between the end effectors and their grasp points 5 : 




Uc&j,' 

*Ci 

*c 2 


(6.25) 


5 Assume that the end effectors have pin connections, so that there are no rotational motion constraints at the end effectors. 
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where are the generalized speeds associated with the object, and xci an d xc 2 are t ^ ie motion 
constraints for the two manipulator end effectors. The associated generalized Jacobian is: 


J( q) = 


^4x4 


1 0 0 
0 1 0 
0 0 1 


J c '(q) 

■7 c '(q) 


( 6.2 6 ) 


which utilizes the constraint Jacobians of Equation (6.17). 

Since the relative velocities between each end effector and its grasp point is constrained to be 
zero, the controller simply sets the last four elements of the desired task-space position, velocity, and 
acceleration vectors— yjj”\ and y^—to zero. 


6.3 Force Constraints 

In addition to motion constraints, there are constraint forces at the manipulator end effectors. The end 
effectors must exert forces on the object to make it move. This section determines the nature of those 
constraint forces. 

Figure 6.3 shows the two resultant forces at each grasp point. The endpoint exerts a force f^» 
on the object at and the object exerts a force f Qx on the endpoint Qi . These forces are equal but 
pointing in opposite directions: 

f Qi = (6.27) 

Analogous relationships exists for constraint moments m. These constraint forces and moments can be 
expressed as generalized constraint forces: 

F C ,=, £ [**]'«*+ £ Kf m ' <“ 8 > 

all all 

constraint constraint 

forces k moments / 

Combining the two forces at each grasp point halves the number of terms in its sum, and combining 
the two moments at each grasp point points also halves the number of terms in its sum. These are 
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Figure 6*3: Force Constraints 

Qi is a point coincident with theith end effector, a nd Q[ is coincident with the ith grasp point 
on the object, is the force on the end effector passing through Qi, and is the force on 
the object passing through Q[. The constraint force Fc, can be expressed in terms of the same 
generalized Jacobian TT Cx utilized for the constraint velocities. 


indicated by the changes in the summing indices from k to i and l to j: 
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(6.29) 


The generalized Jacobian components, J Vi (q) and in Equation (6.29) are the same ones 

that appear in the velocity and angular velocity constraints (see Equation (6.18) and Equation (6.23)). 

Hence, for a system with N end effectors, the generalized constraint force can be written in matrix 
notation as: 

fQi 


F c = , 

(6.29) L 


J c '( q) ... J Cp ( q) JZ'{ q) ••• ^w'(q) 
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end i 
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m 


end i 


(6.30) 


Define the constraint Jacobian J c ( q) as the first matrix of Equation (6.30): 


J c { q) = 


J Cx { q) ... J Cp (<i) q) ••• ^*(q) 


(6.31) 


Those terms corresponding to end-effector degrees of freedom that are not constrained may be dropped. 
Example 

For the example in Figure 6.3, the generalized constraint force vector is given by: 

(6.32) 

where JJ Ci (q) and JT Cl (q) are the constraint Jacobians for the two end effectors, and f^ 1 and f^ 2 are 
the end-effector forces. 
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6.4 Force and Torque Mapping 

The taf£-space adaptive controller of Equation (5.8), however, cannot be implemented direcdy with 
multiple-manipulators utilizing system concatenation. One small modification remains. Because system 
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concatenation model represents the payload object as a separate system, the commanded generalized 
active forces resulting from the control law contain elements that correspond to forces and moments 
on the object. Since the object does not have actuators of its own, these forces and moments must be 
supplied by the manipulators. An additional mapping is required to map the commanded forces and 
moments on the object to commanded manipulator forces and moments. 

Consider the two planar arms and object in Figure 6.1 as an example. The generalized active force 
for the system is (see Equation (6.1 1)): 

F arrri\ 

^ F armi 
^ obj 

The generalized forces on the object, represented by F^-, must be mapped back to the generalized 
active forces corresponding to the manipulators, F armi and F orm2 . 

This section develops this force and moment mapping by explicitly modelling the constraint forces 
and moments at the manipulator end effectors. This approach first solves for the required end-effector 
forces and moments required to generate the requested force and torque on the object resulting from the 
control law, it then uses the result from Section 6.3 to transform the end-effector forces and moments into 
generalized constraint forces. The sum of the generalized active forces and the generalized constraint 
forces forms a pseudo-generalized active force vector T such that the system concatenated model in 
Equation (6.3) can be used directly in the task-s pace adaptive control law, Equation (5.8), with F 
replaced by T. 

6.4.1 End-Effector Constraint Forces and Moments 

The required end-effector forces and moments needed to apply the desired force and torque on the 
object is not unique, as the free-body diagram in Figure 6.4 illustrates. The vector R € R 3 represents 
the resultant of all the end-effector forces and acts on a line passing through the center of mass of the 
object 6 , and T € R 3 is the total torque placed on the object by the end-effector forces and moments. 
At each grasp point Q [ , there may be a force f* € R 3 and a moment m » € R 3 , and each p* € R 3 
represents the position vector from the center of mass to the associated grasp point Q[. 

6 If the task-spice control objective is to control the position of some other point P on the object, then the analysis uses 
the point P rather than the center of mass. P is commonly known as the remote center of compliance[$2, 49]. 


(6.33) 
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Figure 6.4: Object Free-Body Diagram 

R is the resultant of all forces acting on the object whose line of action passes through the 
center of mass of the object, T is the total torque on the object, f t and m* are the forces and 
moments applied at point Q', and p t is the position vector from the center of mass to point 
Q'. The vectors a\, a 2 , and a 3 are mutually orthogonal unit vectors fixed in the inertial frame. 


For a manipulator system with N end effectors, the resultant force R and the torque X can be 
related to the end-effector forces and moments as: 
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(6.35) 


where Pi 6 R 3x3 is the cross product matrix, 


Pi = 


0 -pi • a 3 pi • a 2 
Pi • a 3 0 — pi • ai 

-Pi • a 2 Pi • ai 0 


(6.36) 


Equation (6.35) represents an under-constrained set of equations, such that there is no unique inverse 
mapping from the resultant force and torque to end-effector forces and moments. 



88 


Chapter 6. System Concatenation 


Let the W € IR 6x6jV denote the mapping matrix: 

13x3 ••• 13x3 03x3 03 x 3 

Pi .. P* ^3x3 ••• 13x3 

and let € JR 6 ^* 6 , the weighted pseudo-inverse of be: 

Wlj 4 A-'W^- (w^A^Wy 1 



(6.37) 


(6.38) 


where A € R 6Nx6N is any weighting matrix and represents the desired manipulator load distribution. 
When A is the identity matrix, the solution weights all manipulators equally. The general solution for 
end-effector forces and moments now can be expressed as: 


mi 




R 

T 


+ (l6N*6N - ) f £ 
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(6.39) 


m N 


where f aux € R 6iV is any optional auxiliary input. Those terms in Equation (6.39) corresponding 
end-effector degrees of freedom that are not constrained may be dropped. 

The premultiplier of f aux chooses the portion of f aux in the null space of ; this term represents 
internal loading on the object that generates no motion. Usually, this entire term can be replaced by 
fint that is chosen to represent the desired internal object “squeeze” forces. 

Utilizing the generalized constraint force of Equations 6.30 and the constraint Jacobian of 6.31, 
Equation (6.39) can be expressed as: 
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(6.39) 


J° T (q) I 


t 



(6.40) 


Hence, given the desired generalized active forces for the payload computed by the task-s pace control 
law (Equation (5.8)), Equation (6.40) produces the equivalent set of generalized constraint forces. This 
relationship is used in the following chapter to round out the development of the complete tasksp&ce 
adaptive controller. 



Chapter 7 


Task-Space Adaptive Controller 


Combining system concatenation and task-space control into the same framework results in a very 
general adaptive controller for rigid-link robotic systems that can contain any number of cooperating 
manipulators and payload objects, operating in any number of control modes. This chapter formally 
presents the control and adaptive laws of the task-space adaptive controller, and summarizes its properties. 


7.1 Control Law 


Utilizing system concatenation, partition the robot system model into M actuated subsystems denoted 
by the subscripts arm* and m unactuated subsystems denoted by subscripts obji. The system model 
can be expressed as 1 : 


F + F c = M'(q)u + C'(q,u)u + G'(q) 


( 7 . 1 ) 


0 = xc 


( 7 . 2 ) 


'This control framework is equally applicable for simpler robotic systems. For a single-manipulator system, there would 
be only one partition and no constraint equations. 
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where xc € Ifv are N motion constraints and the partitions are: 
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( 7 . 4 ) 


( 7 . 5 ) 


( 7 . 6 ) 
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C'(q,u) = 
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Additionally define the pseudo-generalized active force 3- to be: 


T = F + F c 
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(7.7) 


(7.8) 


(7.9) 


A 


r = W T (q) 


(r- J cT Wnt) 


(7.11) 


The control law for the task-s pace adaptive controller now can be expressed as: 

T = M^qJuj + C'(q, u<f)ud + G ; (q) + ft + J7 r (q) ^Ky^y + Kp y y task J (7.10) 
I M*M -^ cT (q)W^. 

0 I mXm 

where y task includes the motion constraints Xc and the corresponding terms for the constraints in 
the desired task - space trajectories — and y f f 8k — are set to zero. See Section 7.3 for the 
derivation of the actuator mapping of Equation (7.11). The control law can be expressed in terms of 
the parameter vector as: 

^ ( =Y'(q, u,,, u d) u d )e + f t + J T ( q) (k^ + K Ps ,y“” fc ) (7.12) 

The adaptive update law is: 

d = rY' r (q, U d , U d , u d )J-\q) (p task + cy‘“ 8 *) 


(7.13) 
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Figure 7. 1 : Block Diagram of the Task-Space Adaptive Controller 

The task-space adaptive control tracks task-space trajectories by performing feedback directly 
on the task-space errors, y ta,k . The adaptive parameter update is also based on the task-space 
tracking error. This controller structure differs from the single-manipulator task-space adaptive 
controller, Figure 5.1, by the more sophisticated actuator mapping, given by Equation (7. 1 1). 
The desired internal forces on the payload, f tnl , also may be specified. Generalized Jacobians 
transform the task-space values into equivalent generalized speeds values. 


Figure 7.1 shows the block diagram of the complete task-space adaptive controller, incorporating 
multiple-manipulator control. The grayed manipulator system blocks indicate that the task-s pace 
adaptive controller is capable of handling any number of manipulators and objects, and they may be 
added to the system at any time, as long as the associated Jacobians are updated accordingly. The 
cooperative manipulator also requires a more sophisticated force- and moment-mapping block given by 
Equation (7.1 1) to derive the actuator forces and torques from the control law. This actuator mapping 
reduces to the identity matrix when the robot is not performing payload object manipulation, so this 
controller structure is a superset of the single-manipulator tasks pace adaptive controller of Figure 5.1. 
The internal “squeeze” force, fi nt , on the payload object may be specified, a most valuable capability 
when handling fragile objects. 

The block diagram also indicates that switching control modes is a simple matter of switching to the 
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appropriate Jacobian and the appropriate force- and torque-mapping blocks. The valid control modes 
can range from single manipulator joint control to cooperative multiple-manipulator and multiple- 
payload object control from the free-flying base of a space robot. 

7.2 Controller Properties 

The following items summarize the features of the task r-space adaptive controller. 

• Task-space control The taskspace is an abstraction of control modes. The controller changes 
control modes by changing the definition of the task-space control vector. Thus, the task-space 
controller is capable of handling any number or mixture of control modes from cooperative 
multiple-manipulator control on a free-flying base to single-arm joint control. The feedback is 
performed directly in the chosen task-space to minimize task-space trajectory tracking errors. 

• Inverse-dynamics controller . The task-s pace adaptive controller is a model-based controller. The 
“Inverse Dynamics” block compensates for the inertial effects and the nonlinear Coriolis and 
centrifugal force terms. The adaptive controller updates this inverse dynamics model to minimize 
trajectory tracking errors. For closed-kinematic-chain systems, system concatenation is utilized to 
simplify the inverse-dynamics model. 

• Tracking-error adaptive control The parameter vector 0 used in the inverse-dynamics block of 
the controller is adaptively updated. The parameters, and thus the plant model, are updated to 
minimize tasks pace trajectory tracking errors. 

• Separable adaptation parameters. For complex systems with multiple subsystems, system con- 
catenation modelling keeps separate the adaptable parameters of each subsystem. Doing so 
allows separate adaptation of each subsystem without sacrificing the capability for simultaneous 
adaptation of the entire system. 

• Generalized Jacobian . The generalized Jacobian is the key to task-space control. Any quantity that 
satisfies the generalized Jacobian relationship (see Equation (5.7)) can be utilized as a task-space 
control vector. Changing control modes requires changing the generalized Jacobian blocks to 
match the new task-space control vector. 
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• Actuator mapping. For implementation, an additional mapping is required to transform general- 
ized active forces — or pseudo-generalized active forces for closed-kinematic-chain systems — into 
actuator forces and torques. 

• Controller insensitivity to sensor noise. As with the original joint-space adaptive controller, the new 
task - space controller is insensitive to sensor noise during regulation. Because the the inverse- 
dynamics feedforward block utilizes desired, not measured, trajectory velocities and accelerations, 
sensor noise does not produce unwanted feedforward actuation. 

• Adaptation insensitivity to sensor noise. Since the adaptation update law also utilizes the regressor, 
Y(q, Uj, Urf, \lj), that contains desired velocities and accelerations, it is not sensitive to sensor 
noise during regulation. The parameters do not drift. 


7.3 Actuator Mapping 


The actuator mapping utilizes the generalized-constraint-force expression of Equation (6.40) to replace 
F c in the expression for the pseudo-generalized active force T\ 
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where is the matrix of constraint Jacobians defined in Section 6.2. The inverse relationship 

needed in the control law is 2 : 


r = W 2 (q)F 
= W T (q) 


ImxM -J rCT (q)w] 6i 
0 Imxm 


[T-J cT {c i )i xnt ) 


7.4 Conclusions 


(7.15) 


The system concatenation concept furnishes to the task-s pace adaptive controller the ability to control 
multiple cooperating manipulators. Moreover, it does so in an efficient manner by not requiring the 
complex closed-kinematic-chain equations of motion. Cooperative manipulator control incorporates 
motion constraints at the end effectors into the taskspzce control vector, and utilizes the explicit 
modelling of end-effector constraint forces and moments to generate a mapping from generalized active 
forces and generalized constraint forces to actuator forces and torques. 

The resulting control law equations are of higher order than those resulting from the use of the 
closed-kinematic-chain equations of motion, but the simplicity of the block-diagonal nature of the 
concatenated system equations more than offsets the disadvantages of the increased order. Additionally, 
the generalized Jacobians tend to be sparse matrices, and sophisticated matrix computation algorithms 
can take advantage of that to reduce further the computational burden. Finally, the use of system 
concatenation does not prevent the tasks pace controller structure from handling simpler single-arm 
control: All that is needed is the appropriate generalized Jacobian. 


2 The following matrix relationship is true for any matrix W E lR nx m : 
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Chapter 8 


The Experimental System 


This chapter briefly describes the experimental system. More details can be found in Ullmans thesis[43]. 

Adaptive control of a free-flying space robot motivated the development of the new task-space 
adaptive control framework. Therefore, the tosk-s pace adaptive controller has been implemented on 
the Multi-Manipulator Free-Flying Space Robot experiment in the Aerospace Robotics Laboratory 
(ARL) for verification. Utilizing air-bearing technology, this facility has pioneered the research in space 
robotics by simulating the drag-free conditions of space with high fidelity in the laboratory[43, 19, 1]. 

The Multi-Manipulator Free-Flying Space Robot was designed to meet some very specific goals: 

• The robot must be completely self-contained and free-flying, containing all the key components 
to be found on a real space robot. 

• The space robot must be capable of autonomous operations. 

• User interaction with the robot must occur at a high level to ease the burden on the human 
operator. 

These goals resulted in a relatively sophisticated experimental system, of which adaptive control is only 
a small part. 

Because of the system complexity, modularity in both hardware and software design became a 
necessity. 

The Multi-Manipulator Free-Flying Space Robot is self-contained, containing its own pressurized 
gas system for floatation and propulsion. It incorporates a pair of arms for performing cooperative ma- 
nipulation. The robot contains NiCad rechargeable batteries for use in free-flying experiments. Power 
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electronics can distribute electrical power utilizing either the batteries or external power. Additional 
analog electronics handle sensing and actuation, as well as on-board battery recharging. The robot 
also has a camera for on-board vision sensing. The on-board computer system provides the “brains” 
for the space robot and is the core of the controls system; the computers also provide on-board vision 
processing. Analog- to- Digital (A/D), and Digital-to-Analog (D/A), and Digital Input/Output (DIO) 
boards furnish the interface between the computers and the sensors and actuators. A wireless Ethernet 
module allows communication with the user and off-board computers 1 . 

Software modularity and the need to provide a high-level user-interface resulted in a hierarchical 
control approach. At the top level is a graphical user interface that allows the operator to view the 
robot and payloads and to direct robot actions via simple mouse motions and simple commands such as 
“move”, “capture”, or “release.” At the heart of the control system is a strategic controller that processes the 
user input and schedules the necessary control mode changes based upon user and sensor inputs. The 
adaptive controller sits at a lower level, along with the trajectory generators. These too are modularized 
such that different controllers, trajectory generators, Jacobian calculations, etc., may be swapped in 
dynamically at the request of the strategic controller or the user. At the lowest level are the sensor and 
actuator nonlinear-compensation calculations. 

8. 1 Hardware Architecture 

The Multi-Manipulator Free-Flying Space Robot is a completely self-contained and modularly designed 
robot capable of fully autonomous operation. The space robot floats on a cushion of air atop a 9 foot 
by 12 foot granite surface plate that is flat to within 0.001 inch between any two points on its surface. 
This results in extremely low friction, providing an accurate representation of the drag-free conditions 
of space in two dimensions. The robot operates in a plane perpendicular to the gravitational force; thus 
gravity is not a factor. 

The space robot carries on board a pair of manipulators, a pressurize gas system for propulsion, 
batteries and electrical power systems, a complete set of sensors include a camera for vision sensing, 
a computer system, and a high-speed wireless communications system. Figure 8.1 shows a picture of 
the space robot 2 . The space robot is functionally separated into three layers. At the bottom is the 

^though high-speed communication is not required for user commands during operation, it greatly enhances controller 
development capabilities. 

2 The space robot was designed mainly by Marc Ullman and Ross Koningstein of the ARL. The vision system hardware — the 
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pressurized gas system. The spherical tanks, capable of holding 3000psi of compressed air, provides the 
gas supply for both flotation and propulsion. The middle layer contains the batteries, battery-charging 
system, power electronics, and analog electronics for the sensors. The top layer contains the computer 
system and digital electronics. It also holds the wireless Ethernet module. At the very top is the vision 
camera, sitting on its boom, that provides local sensing of the manipulator end effectors and of the 
payload objects. Finally, the pair of planar manipulators reaches out from the front of the robot. 

The base of the space robot measures 500mm in diameter and, neglecting the camera boom, and 
800mm in height. It has 65kg in mass and a moment of inertia about its center of mass of 3.2kg-m 2 . 
The maximum reach of each arm is 600mm, and the mass of each arm is 2.3kg. Detailed mass 
distribution can be found in the initialization files in Appendix E. 


8.1.1 Actuators 

Free-flying base actuation is provided through eight gas-jet thrusters. The thrusters are mounted 
between the two bottom layers on the corners of a square whose diagonal is a diameter of the base (see 
Figure 8.2). The thrusters may be fired in combinations to produce translation and rotational morion. 
An optimal bang-off-bang thrust-mapping algorithm for the on-off thrusters is used to approximate 
linear control [43]. The pressure for the thrusters is regulated nominally at lOOpsi, and each thruster 
supplies IN of force. Therefore, the maximum force that can be applied in any one direction is 2N, 
and the maximum torque is 1 N-m. 

The manipulators are in a SCARA configuration, each with two degrees of freedom. The actuators 
are brushless DC limited-angle torqucrs 3 mounted at the robot base to minimize link inertias and to 
prevent the center of mass of the robot from shifting away from the center of the air bearing. Each 
shoulder is connected directly to its motor, and each elbow is connected to its motor through a cable 
drive to eliminate backlash. Compensation for the nonlinear torque curve versus angular position 
is done in software 4 . The maximum torque from each shoulder motor is about 0.8N-m, and the 
maximum torque from each elbow motor is around 0.5 N-m. 

Each end effector is a pneumatically-actuated “gripper” that moves in the vertical direction on linear 
Point Grabber II — is designed and built by the author. 

The motors are manufactured by Aeroflex. Model V40Y-6H is used for the shoulders and model V40Y-5H is used at the 
elbows. 

The torque curve is modelled as a fourth-order polynomial constrained to have only one inflection point at the motor 
angle with maximum torque output. 




Figure 8.1: Multi-Manipulator Free-Flying Space Robot 

The Multi-Manipulator Free-Flying Space Robot is a fully self-contained autonomous robot 
with on-board fuel, power, computers, vision, and wireless communications systems. 
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Figure 8.2: Thruster Arrangement 

The space roboc carries eight gas jet thrusters , mounted on the four corners of a square in the base 
of the robot . Two thrusters at each face of the square can provide pure translational motion , and 
when fired in combination, the thrusters can supply both rotational and translational motions 
simultaneously. The numbered pairs of thrusters provide positive and negative thrust along 
the indicated line of action. An optimal bang-off-bang thrust mapping is used to approximate 
linear control. 

bearings. To grasp a payload object, the “grippers” are lowered into mating “gripper ports” in the 
payload. A bearing at the tip of the gripper prevents the gripper from applying a torque on the object, 
and an O-ring around the bearing ensures a snug fit in the gripper ports. 

8.1.2 Sensors 

The manipulator-joint- angle sensors are analog RVDT s (rotational variable differential transformers) 
mounted on the motor shafts. An additional analog filter provides band-limited pseudo-angular 
rate. The RVDTs are Pickering model 23501-0, each having a range of 150 degrees. The RVDT 
position signals are additionally passed through a third-order polynomial in software to compensate for 
nonlinearity. 

Other sensors provide information on battery voltage and gas tank pressure. These are used 
during initialization to determine the health of the space robot system. They can also be monitored 
continuously during operation to determine when the robot should return to its dock for recharging 
and refueling. 
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8.1.3 Vision Subsystem 

The position sensors for the space robot and payload objects are vision-based. A vision system consists 
of a Pulnix 440S CCD camera, a Point Grabber II vision-processing board 5 , and a computer board to 
process the vision data and to run the VisionServer software 6 . The VisionScrvcr utilizes bright targets 
to identify and track named objects. Three targets on each object are sufficient for identification and 
for providing position and orientation information in two dimensions 7 . Objects of interest are marked 
by bright targets using either infrared LEDs or highly-reflective discs. The PointGrabber locates bright 
pixels in the cameras field of view and stores their positions. Postprocessing assembles the bright pixels 
into groups corresponding to the targets before passing the information to the VisionServer. 

The combination of vision hardware and software can provide at 60Hz resolution of better than 
a 1/20^ of a pixel; for a field of view of two meters, this resolution translates to about 0.25mm. 
Third-order polynomials correct for the wide-angle-lens distortions to provide good accuracy over the 
entire field of view. 

Two vision systems provide the global positioning information over the entire surface of the granite 
surface plate 8 . The cameras are mounted above the table, and two off-board computers perform the 
vision processing. The global position and velocity information is sent to the space robot via the wireless 
Ethernet link. The space robot base and each payload object are equipped with a triad of infrared LED s, 
mounted in a unique pattern. These patterns are registered with the VisionServers, allowing them to 
identify and track each body. 

The space robot also contains an on-board vision system for local sensing. Because the field of view 
is smaller, this vision system provides much higher resolution. The local vision system also enables 
high-speed local feedback, and is not affected by possible communication delays and drop-outs from 
global positioning systems. Each manipulator end effector also carries an infrared LED, allowing the 
on-board vision system to track the endpoints for endpoint control 9 . 

5 See Appendix B for more details on the Point Grabber II. 

6 The VisionServer software was developed by Stanley Schneider at ARL. 

7 Ongoing research at ARL is investigating the use of five targets per object for acquiring the position and orientation in 
full three dimensions. 

8 This “surrogate** global positioning system most likely will be replaced in space by some combination of inertial navigation 
system and GPS receivers. 

9 Endpoinc sensing is of course absolutely essential for capturing payloads, because the controller must match the positions 
and velocities of the end effectors with those of the gripper ports on the payload. 
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8.2 Electrical Subsystem 

Electrical power is needed to operate the manipulators, the computers, and communications. A fully 
self-contained power subsystem, all the analog and digital electronics, and the computer systems for 
control are contained on board the space robot. 

8.2.1 Analog Electronics 

Analog circuitry is needed to supply electrical power to the robot system, and to process the signals 
from analog sensors. 

On-board rechargeable NiCad batteries provide electrical power to the the space robot when it is 
free-flying. The batteries can supply ±12VDC at up to 15 Amps. Connecting external power when 
the space robot is docked also provides electrical power and simultaneously recharges the batteries. 
Off-board charging is available as well, and the battery packs can be replaced while the space robot is 
“live”. The batteries may be engaged and disengaged via manual toggle switches or through computer 
control. Figure 8.3 shows a simple schematic of the analog electronics subsystem. 



Figure 8.3: Analog Electronics Subsystem 


The motor drivers use the raw, unregulated, power from the electrical bus. Power converter units 
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provide regulated power at ±5V to the computer and digital electronics, ± 12V and ± 1 5 V to the analog 
electronics subsystem, and +8V to the wireless Ethernet module. 

The analog electronics subsystem contains the main power control unit (PCU), battery chargers, 
safety disconnect circuitry, sensor electronics, and motor drivers. The PCU contains the master power 
switch and the battery engaging switches 10 . It is the main power-distribution center, allowing any 
combination of batteries and external power to drive the power bus. The battery-charging circuits 
provide three charge rates and include automatic switching into trickle charge as the batteries reach 
their maximum voltage. The thrusters and arm motors arc enabled through the safety disconnect switch. 
In addition to the manual enable switch, the controller must enable the switch explicitly during every 
sample period; otherwise, the safety switch kicks in to disconnect power to the motors and thruster 
relays. This effectively kills power to all actuators if the control system fails. 

The sensor electronics contain the excitation circuitry and filtering for the RVDTs used for 
manipulator-joint-angle sensing. It also supplies power to the infrared LEDs at the end effectors. 
A multiplexer board takes sensor readings from the battery voltages, gas tank pressure, and the regulated 
pressure. Finally, the motor drivers supply current to the arm motors. 

8.2.2 Computer Subsystem 

The real-time computer systems used in ARL are based on the VMEbus. The VMEbus is widely 
supported in industry with both hardware and software products. Using standardized products shortens 
development time and ensures robust, well-debugged computer components. Figure 8.4 show a simple 
schematic of the computer system. 

The main on-board computer is a Motorola MV167 single board computer containing a 25MHz 
MC68040 processor. This computer handles all the task-space adaptive control calculations, high-level 
strategic control, trajectory calculations, path planning, sensor and actuator signal conditioning, and 
nonlinear compensation of sensor and actuator signals. Another computer, the Motorola MV133- 
1 with a 16.67MHz MC68020, performs all the calculations for the on-board vision system. Two 
additional off-board computers, Motorola MV147 20MHz MC68030 processor boards, handle the 
global vision system calculations. The on-board computer communicates with each other via the 
VMEbus backplane, and communication with the off-board computers utilizes the wireless Ethernet. 

10 The batteries may be engaged manually or through computer control. The switches automatically disengage when the 
batteries are removed. 
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Figure 8.4: Computer Subsystem 


Each of the vision-processing computers communicate with a Point Grabber II board. 

The wireless Ethernet is the Motorola ALTAI R system. It consists of a Command Module and 
multiple User Modules. Each module appears as a “node” on the Ethernet, and communicate with each 
other at 19GHz microwave frequencies with an effective throughput of over 3Mbits/sec. Each module 
contains a six-sector antenna, and each continually monitors all signal paths to determine the best signal 
and to reject multipath signals, resulting in a very robust Ethernet connection in the laboratory setting. 
The Command Module is direcdy connected to the laboratory network, and a User Module is placed 
on the space robot. Additional User Modules are placed on other space robot experiments in the same 
laboratory[10]. 

The Xycom XVME590 16 differential channel 12-bit Analog-to-Digital (A/D) and the Xycom 
XVME595 4 channel 12 bit Digital-to-Analog (D/A) boards translate sensor input and actuator output 
to and from digital format used by the computer. A Xycom XVME290 digital input/output board 
furnishes a user-settable clock that is used for the sample clock. It also operates the on-off thrusters, 
the grippers, and the sensor multiplexers so the computer may monitor the battery voltages, gas tank 
pressure, and regulated pressure. 
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A SUN workstation completes hardware requirements for the space robot experiment. The work- 
station is the software development center for the space robot. All the run-time code is written and 
compiled on the workstation before sending it to the real-time computers. The workstation also runs 
a graphical user interface for interacting with the space robot. The workstation additionally is used for 
data collection and analyses. Because the space robot is hooked up to the Ethernet, any workstation — or 
multiple workstations — may be used for these purpose. 

8.3 Software Architecture 

The software architecture is developed to enable the user to interact with the space robot at a high task 
level, such as directing the robot to chase down and capture a free-flying object with a click of a button. 
The software is divided into three major areas; the graphical user interface, the strategic controller, 
and the dynamic controller. The graphical user interface runs on the workstations and communicates 
with the real-time computer systems to acquire position and orientation information for display and 
to send commands to the space robot. The strategic controller and the dynamic controller both run 
on the main real-time computer on the space robot. The strategic controller takes user inputs and 
schedules the necessary controller mode changes to carry out the task; the mode changes may occur at 
set time intervals or upon satisfying some conditions based on sensor inputs. For example, when the 
end effectors have been tracking the payload object grip ports to within an error tolerance, the grippers 
are engaged, and the control switches from end-point control to object control. The dynamic controller 
includes all the inverse-dynamics, adaptive update, Jacobian, and trajectory calculations. Both the 
strategic controller and the dynamic controller are implemented in the Controlshell real-time software 
environment developed by Stanley Schneider of ARL 1 1 [28, 34, 43]. 

8.3. 1 Graphical User Interface 

The graphical user interface (GUI) is the users link to the space robot. It displays the global position of 
the robot and other objects of interest, and it allows the user to issue simple commands such as “move,” 
“capture,” and “release.” This furnishes the desired high-level interface and disencumbers the operator 
from the burden of performing continuous hand-in-glove-type teleoperations. 

1 1 Extensions to ControlsheU have been implemented by Marc Ullman and the author. 
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The graphical user interface communicates directly with the global-vision-system computers to 
acquire the global positions of the robot and the payload objects for display. Since the user is not in the 
high-speed feedback loop, the position display need not occur at high speed; high-performance control 
is immune to communications delays 12 and graphical display delays. 

Figure 8.5 shows a portion of the graphical user interface. The space robot is indicated by the 
circle with the cross; the arrow on the cross shows the robots heading. The payload object is similarly 



Figure 8.5: Graphical User Interface 

The space robot is represented by the circle with the cross-hairs . The payload object is 
represented by the oval with cross-hairs that has the solid outline. The ghost image of the 
object , represented by the dashed outline , shows the desired payload position. The command 
buttons for "move, * "capture, ” "release, n and “reset” appear in the top portion of the interface. 
Adaptation may also be enabled through a command button. The manipulators are not shown, 
because their configurations are not essential for the user. 


displayed, but with an oval pattern. The solid outlines indicate the actual positions of the objects, while 
a dotted outline — known as the “ghost image” — represents the desired position and orientation of the 


12 Comm uni cations delay is insignificant in the laboratory, but is an issue with real robots in space. 
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object. The human operator moves this ghost image employing a computer mouse. When the ghost 
image has been placed at the desired location and orientation, the operator clicks on the “move” button 
to initiate the action. If the object being moved is the space robot, the control system will move the 
robot base to the desired location using thrusters. If the object to be moved is the payload, the robot 
will first capture the payload, then move the payload to the desired location. The operator may also 
issue separate commands to “capture” and “move” the payload. Clicking on “release” directs the space 
robot to release a payload, and clicking on “reset” resets the robot to a known state. Additionally, the 
user may enable or disable adaptation through the graphical user interface. 

Because the user directly commands the position and orientation of the robot or payload, he need 
not be concerned with the manipulator configurations; teat-space control and the strategic controller 
manage all the detailed arm manipulation maneuvers. Accordingly, the manipulators are not displayed 
in the user interface. 

8.3.2 Strategic Controller 

The strategic controller is the heart of the controls system and is implemented using state-table program- 
ming [32, 43]. A “state” represents the current robot activity, and is typically one step in the execution 
of a requested task. State transitions are triggered by external stimuli; each state determines which 
stimuli to monitor. When a stimulus occurs, the strategic controller executes the associate transition 
routine, and the result from the transition routine determines the next state. 

A stimulus may be an edge-triggered or a persistent event. Examples of edge-triggered stimuli are the 
expiration of a timer and the completion of a trajectory. These stimuli are active only for an instant. If 
they happen when they are not being monitored by the current state, they are lost. Persistent stimuli 
typically indicate status, such as the found/lost status of an object or the up/down status of the grippers. 
By being persistent, a state need not be monitoring the stimulus when it changes; it also enables a state 
to combine stimuli to form a new one, for example “GrippersDown” is formed by “RightGripperDown 
AND LeftG ripper Down.” If “RightGripperDown” and “LeftGripperDown” were both edge- triggered 
such a combination would not be possible without forming intermediate states. 

State-table programming can be illustrated graphically by a state-transition graph, such as the 
example in Figure 8.6. This example is a simplified state-transition graph for the capture of a payload. 
The robot starts in the “Ready” state. When it receives the “Capture” command from the user interface, 
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This state transition graph example illustrates the basic components of the strategic controller 
in performing a payload capture. 


it executes the “StartCaptureO” transition routine, and ends up in the “Starting Capture” state. If the 
object is not within the view of the local camera, but it has been found by the global positioning system, 
the robot plots a trajectory for the robot base to rendezvous with the object and enters the “Moving 
Base” state. If when the base trajectory has been completed but the object has not come into view yet, 
the robot re-enters the “Starting Capture” state to plot a new base trajectory. When the object comes 
into view of the local camera in either the “Starting Capture” or “Moving Base” state, the robot runs 
the “CheckRangeO” routine to determine if the object is within the reach of the arms; if out of range, 
the robot enters the “Waiting” state; otherwise, it proceeds to the “Slewing to Object” state. While 
in “Waiting”, the robot continually checks for the object to come into reach before transitioning to 
“Slewing to Object.” ♦ 

“Slewing to Object” places the manipulators under endpoint control to command the arms to 
follow a trajectory to intercept the grip ports on the payload. When both grippers reach the grip 
ports, “ArmsAtObject” triggers the transition to “Tracking”, where the grippers must track the payload 
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grip ports for a set period of time to ensure that the grip tolerance is met. If at the end of the time 
period — indicated by the “TrackTimer” stimulus — the tolerance is not met, the manipulators must 
continue to track the grip port; otherwise, the robot lowers the grippers. When both grippers are 
down, the robot employs object control 13 to execute the “StopObjectO" transition routine and enters 
the “Manipulating” state. If the robot receives the “Release” command while “Manipulating”, it raises 
the grippers and returns to the “Ready” state to await further commands. 

Error handling is easily implemented with a state-table programming. In any state before the 
payload has been grasped, an “ObjectLost” stimulus from the vision system can bring the robot into 
an “Error” state 14 , and eventually back to the “Ready” state. If, while manipulating the object, 
the “GripToleranceExceeded” stimulus occurs — indicating that the robot may have lost grip on the 
object — the robot may release the object and attempt to reacquire it. 

Similar state transition graphs have been developed and implemented 15 for processing other user 
commands to move the robot base, move the payload, and enable adaptive control. 

8.3.3 Controlsheil 

Controlshell is a software framework developed by Stan Schneider at ARL that aids the development 
of real-time control systems. Controlshell enables the user to develop a multiple-processor hierarchical 
control system in a modular fashion. It directly supports state-table programming utilized by the strategic 
controller. At the lower level, Controlshell incorporates user-defined components for performing any 
number of control-system tasks ranging from acquiring sensor values to computing the task-space 
control and adaptive update laws. Controlshell also allows definitions of complete configurations of 
components — configurations that comprise all the components that pertain to a control mode. These 
configurations may be swapped dynamically to effect control-mode changes under the direction of the 
strategic controller or the user. Please see [34, 43, 28] for more details on Controlshell. 

The task-space adaptive controller is implemented at this software level. Different Jacobian com- 
ponents are defined to correspond with the different task-space control objectives representing different 

13 Object control, endpoint control, and joint control are all implemented with the task-spucc adaptive controller. 

u When the object is being grasped, the robot may infer the object location from its joint sensors and knowledge of the grip 
port locations on the payload, so the vision system losing the object should not cause the robot to abort manipulation. The 
grip is also monitored to determine if the robot is grasping the payload. 

15 The state-transition tables were originally developed by Marc Ullman of ARL [43]. The author made modifications and 
added capabilities to handle adaptive control. 
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control modes. Appropriate torque-mapping components are also defined. There is, however, only one 
component for the inverse-dynamics and one for the adaptive update calculations. 

As a side note, the real-time computers run the VxWorks real-time operating system from Wind 
River Systems. In addition to providing facilities for real-time activities, the operating system supports 
networked file systems, allowing each computer to download its run-time code directly from the 
laboratory file systems via the high-speed Ethernet connections. Controlshell runs on top of VxWorks. 

8.4 Payload Subsystems 

Figure 8.7 depicts the two free-flying payload objects used to investigate the performance of the adaptive 
controller. They possess inertia properties that differ by an order of magnitude. These differences are 



Figure 8.7: Free- Flying Payload Objects 

The free- Hying payload objects have mass properties that differ by an order of magnitude, but 
they appear identical to the vision system and the space robot , because the LED patterns and 
grip port locations are identical. 

enough to demonstrate significant deterioration in controller performance were adaptation not used. 
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Both payload objects float employing battery-operated aquarium pumps. Each top plate includes 
three infra-red LED s for identification and tracking by the vision systems. The patterns are identical 
for both objects, such that the vision system and space robot cannot know which object the robot is to 
capture and manipulate. The grip ports on both payloads also are located identically. The smaller object 
is made of honey-combed aluminum, and its mass is 1.0kg and its moment of inertia is 0.007kg-m 2 . 
The larger payload is made of solid stainless steel and masses 8.9kg; its moment of inertia is .lkg-m 2 . 


Chapter 9 


Implementation and Experimental Results 


The new tar£-space adaptive controller was implemented on the Multi-Manipulator Free-Flying Space 
Robot. This controller, coupled with the strategic controller described in Chapter 8, allows the 
space robot to execute complicated control sequences — including the chase, capture, and placement of 
free-flying objects — with adaptation available throughout all control-modes. This chapter documents 
experimental results verifying the performance of the adaptive control for two of control modes, or 
tasks: object control and endpoint control. 

Payload adaptation offers the most dramatic results, since changing payloads causes step changes 
in the system parameters; and the adaptive controller must respond quickly to these step changes 
to maintain good performance. Experimental results of adaptation to two payloads with an order- 
of-magnitude difference in inertial parameters is presented. They show the poor performance of a 
nonadaptive controller when the controller is given the wrong set of payload inertial parameters. They 
also show the improvement when adaptation is activated. The results suggest that adaptive control can 
deliver performance equal to or even better than a nonadaptive controller using nominal payload inertial 
parameters. 

This chapter also shows the capability of the task-s pace adaptive controller to adapt to robot 
parameters. The inertial parameters of the right manipulator are set to zero, and endpoint control is 
enabled. The results indicate that the adaptive controller can also adapt to large changes in the robot 
parameters and thereby greatly reduce the endpoint trajectory errors. 
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9.1 SystemModel 

Figure 9.1 shows the schematic used to model the Multi-Manipulator Free-Flying Space Robot. The 



Figure 9.1: Multi-Manipulator Free- Flying Space Robot Schematic 

The space robot consists of five rigid bodies. Each body i has mass rrii and moment of 
inertia The center of mass of the base is offset from the geometric center of the base by 
(Z/cOx, LcOy). The shoulder of each manipulator j is located at (L c j x > L c j y ), measured from 
the base center. The upper link of each manipulator j is L$\ long, and its center of mass is 
iocated at (Lj\ x > Lj\ y ), measured from the shoulder. The lower link of each manipulator j 
is Lj 2 long, and its center of mass is located at {Lj 2 x, Ljiy)> measured from the elbow. The 
base-relative coordinate system is fixed in the base frame and is centered in the base; the x-axis 
is aligned with the unit-vector b^ and die y-axis is aligned with the unit-vector b 2 . 


space robot is a seven-degree-of-freedom system, consisting of five rigid bodies. The free-flying base 
has three degrees of freedom — two in translation and one in rotation, and each manipulator link has 
one rotational degree of freedom. The mutually perpendicular unit- vectors, bi and b 2 , are fixed in the 
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robot-base frame. The base-relative coordinate system is fixed in the base, and its center coincides with 
the center of the base; the body-fixed x-axis is aligned with bj, and the y-axis is aligned with b 2 . 

The payload object is a single rigid body, possessing three degrees of freedom. Figure 9.2 shows 
the schematic used to model the payload. The space robot and the payload are combined to form a 
complete system model used by the Att£-space adaptive controller. 



Figure 9.2: Payload Object Schematic 

The payload is a single rigid body with three degrees of freedom. It has mass rn^j and moment 
of inertia I 0 bj . The center of mass is located at (L* , L*) f measured from the geometric center 
oftheobject . The grip pons are located at L gl and L g2 along the center line. The body- relative 
coordinate system is fixed in the payload, and the x- and y-axes are as indicated. 


The robot may be modelled with sixteen (16) parameters, and the payload modelled with another 
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four, giving the twenty-term system parameter vector 1 : 

mp + mj + m2 + m3 + m^ 
m 0^cOx + ("M + m 2 ) t'clx + ( m 3 + m 4 j L clx 
m 0 L dby + ( m l + ^ 2 ) Lely + ( m 3 + "* 4 ) L cly 
m l^llx + m l L U 
m l L \ly 
m 2^12x 
m lL 12y 

m 3 L 2 \x + m 2^21 
m 3 L 2 i v 
m 4 L 22x 
m 4^22v 

m O L lo + ( m l + ”*2) + (m 3 + m 4 ) L 2 a + J 0 

m l ( L llx + L \ly) + m 2 L \\ + 7 1 
m 2 ( L 12 x + L 12y) + 7 2 
m 3 ( L 21 x + L \\ y) + m 4 L 2i + 7 3 

m 4 ( L 22 x + + 

m obj 
771 obj L x 
m obj L y 

m 0bj (^X + L x ^ + I 0 frj 


Payload adaptation is demonstrated utilizing base-relative control. This is an important capability 
for a free-flying robot, because local sensors can provide high-bandwidth, high-resolution sensing that 
may not be available from global sensing systems. A typical manipulation task is to extract or insert 
the payload from or into a mating part. It is important for the local sensing system to sense both the 
payload and the mating “port” during these maneuvers. 

To model this situation, a separate “port” object is placed in the field of view of the local vision 
system, and the robot is directed to perform slews to, and regulate at positions fixed in the reference 
frame of the “port”, as illustrated in Figure 9.3. Because the space robot base may move during the 
slews, the “port” is not fixed in the robot s reference frame. With relatively low feedback on the space 
robot base position and orientation, the base is free to move within a bounded area before the on-off 
thrusters fire. This can save precious fuel when performing local manipulation. 

Experimental results are shown for handling of the small and large payloads with and without 
adaptation. Note that only the last four parameters need to be updated for payload adaptation. 

Adaptation to robot parameters is demonstrated with endpoint control: the right manipulator 
endpoint is commanded to follow trajectories in the robot-base reference frame. The inertial parameters 
of the manipulator are set to zero to compare the performance with and without adaptation. All 




, The derivation is left to the reader. 
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Figure 9.3: Payload Slews Relative to Mating “Port” 

This schematic shows a typical slew that the space robot performs. The payload is directed to 
follow a trajectory in the “port” reference frame , in the presence of robot base motions. The 
cross-hairs in the robot base indicate that the robot base may move during the slew. 

the parameters containing the manipulator inertial parameters — mj, m 2 , 1 1 , and I 2 — are adaptively 
updated. 

9.2 Adaptation to Small Payload 

The experimental results for the control of the small payload is separated in four sections: nonadap- 
tive control using nominal payload parameters, adaptive control starting with nominal parameters, 
nonadaptive control starting with incorrect parameters, and adaptive control with incorrect parameters. 

9.2.1 Nonadaptive Control with Nominal Payload Parameters 

The results for the nonadaptive control using nominal payload parameters serve multiple purposes; it 
represents a baseline controller to which the adaptive controller is compared, and it serves to introduce 
the format of the data plots. The payload is commanded to follow back-and-forth slews, illustrated by 
Figure 9.4. This time-lapsed “overhead view” of the system is plotted from actual experimental data. 
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Robot Motion 



X Position (m) 


Figure 9.4: Robot Slew from Actual Data 

This plot depicts a typical slew that the robot is commanded to follow. The View” is taken 
from inertial space. The robot base is under control, but the control gains are adjusted to allow 
small base movements before the on-off thrusters fire; close inspection of the plot shows that 
the base did rotate several degrees. 


Figure 9.5 displays the plots for the actual and desired position and orientation of the payload in the 
“port” reference frame. The lower right plot shows the Cartesian path that the payload followed during 
the slews. These time-histories show that the controller performed reasonably well in trajectory- 
tracking. The steady-state offsets are caused by spring forces from tubing inside the manipulators. 
Integral control reduces these errors, but it has been disabled to compare the performance of the basic 
task-space controller with and without adaptation. 





Orientation (deg) Position (cm) 


9.2. Adaptation to Small Payload 


119 



Figure 9.5: Small-Payload Trajectories — Baseline Nonadaptive Control 

The actual and desired payload trajectories measured in the “port”-fixed reference frame, which 
is fixed in inertial space for the series of experiments presented here . The solid lines represent 
the actual measured payload trajectories of the payloads geometric center and the dashed lines 
represent the desired trajectories. The Cartesian trajectories are in the top two plots, and its 
accompanying orientation is in the lower left plot. The lower right plot shows the “X vs. Y n 
trajectory, representing an “overhead” view of the path traced out by the center of the payload, 
as seen horn a reference frame fixed to the “port”. The oval icons at the ends of the slews show 
the payload orientations at those locations, but the icons do not depict the actual payload — the 
icons are much smaller than the actual payload. 


9.2.2 Adaptive Control with Nominal Payload Parameters 


This section presents results for enabling adaptive control when the payload parameters are at their 
nominal values. Doing so allows comparisons of the adaptive controller performance and the baseline 
nonadaptive controller under near ideal conditions. It also can show how the parameters will eventually 
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converge. 

Figure 9.6 demonstrates that adaptation does not deteriorate the trajectory-tracking performance 
(compare with Figure 9.5). The time-histories of the payload-parameter estimates are shown in 




Figure 9.6: Small-Payload Trajectories — Adaptive Control Starting with Nominal Values for Param- 
eter Estimates 

These actual and desired payload trajectories are measured in the t port”-tixed reference frame. 

They show the results for the adaptive controller starting with the nominal payload parameter 
estimates. The time-histories show no significant difference in performance when compared 
to the baseline nonadaptive controller. The oval icons in the lower-right plot show the payload 
orientation at those locations. 

Figure 9.7. The payload mass direedy corresponds to the parameter 0\ 7 (see Equation (9.1)); the 
center-of-mass locations and the moment of inertia may be solved using the last four parameters of 9. 
The results show that the mass and center-of-mass estimates do hover around their nominal values; but 
the moment of inerua has a larger relative variance around its nominal value. A possible explanation is 
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Figure 9.7: Small- Payload Parameter Estimates — Adaptive Control Starting with Nominal Values for 
Parameter Estimates 

These plots show the time-histories of the parameter estimates when they are initially given the 
nominal values, and then the payload is moved as in Figure 9.5 and Figure 9.6. 


that the spring forces in the arms are masking the inertial effects of the payload, since the true moment 
of inertia of the payload is small (.007kg-m 2 ). Nevertheless, the sharp changes in moment-of-inertia 
estimate do not affect the trajectory tracking performance. 


9.2.3 Nonadaptive Control with Incorrect Payioad Parameters 

The real advantages of adaptive control are evinced when the controller is supplied incorrect parameter 
estimates. This section presents results for the nonadaptive controller controlling the small payload 
when given the payload parameters of the larger payload. Figure 9.8 shows the payload trajectories. 
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Because the controller believes the payload to be an order of magnitude more massive than it is, the 



Figure 9.8: Small-Payload Trajectories — Nonadaptive Control Starting with Incorrect Values for 
Parameter Estimates 

The actual and desired payload trajectories , measured in the u port”-fixed reference frame. They 
show the results for the nonadaptive controller actually controlling the smaller payload but 
starting with the larger- payload values for its parameters. The time history shows that this is 
an unacceptable controller (it skirts instability). 


controller requests too much actuator effort to initiate each slew. The plots demonstrate this by showing 
that actual trajectories lead the desired trajectories at the start of each slew. The feedback portion of the 
controller finally dominates toward the end of each slew to slow the payload, but causes large reversals 
in direction. This is not an acceptable controller. Many times this particular situation leads to violent 
instability. 
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9.2.4 Adaptive Control with Incorrect Payload Parameters 

This time, adaptive control is enabled, with initial payload parameters set to that of the larger payload. 
Figure 9.9 shows a dramatic improvement in performance. The adaptive control updates the parameters 



Figure 9.9: Small-Payload Trajectories — Adaptive Control Starting with Incorrect Values for Param- 
eter Estimates 

The actual and desired payload trajectories, measured in the “port”- fixed reference frame , show 
the results for the adaptive controller actually controlling the smaller payload , but starting with 
the larger-payload values for its parameters. The adaptive control adapts fast enough to prevent 
significant overshoot by the end of the first slew, and effectively recovers the baseline controller 
performance quality by the second and third slews (compare with Figure 9.8). 


quickly enough to prevent significant overshoot by the end of the first slew. By the second and third 
slews, the controller performance rivals that of the baseline nonadaptive controller of Figure 9.5. 

Figure 9.10 displays the time-histories of the parameter estimates. These plots show the rapid 
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Figure 9.10: Small-Payload Parameter Estimates — Adaptive Control Starting with Incorrect Values 
for Parameter Estimates 

These plots show the time histories of the parameter esrimares when they are initially set to the 
parameters of the larger payload. 

adaptation of the payload parameters. The mass and moment-of-inertia estimates converged rapidly 
toward their true values. The center-of-mass estimates, however, approached a location about 6mm 
from the nominal location. This demonstrates once again that, lacking a sufficiendy exciting trajectory, 
trajectory-tracking convergence docs not imply parameter convergence. 

9.2.5 Small-Payload Control Summary 

To summarize the results for control of the small payload, Figure 9.11 shows the trajectory- tracking 
errors of the baseline controller, the adaptive controller starting with the incorrect parameters, and 






9.2. Adaptation to Small Payload 


125 


the nonadaptive controller utilizing the incorrect parameters. The plots show that the adaptive 
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Figure 9.11: Small-Payload Trajectory-Tracking Errors 

These plots show the time-histories of the trajectory-tracking errors of three controllers . The 
adaptive controller already performs much better than the nonadaptive controller on the first 
slew ; and as well as the baseline controller — which is using the nominal parameters — after the 
Erst slew , ; 


controller performed better than the nonadaptive controller, even though they started with the same 
set of incorrect payload parameters. By the second and third slews, the adaptive controller performs as 
well as the baseline controller. 
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9.3 Adaptation to Large Payload 

Adaptation to the large payload duplicates the experiment in Section 9.2 using the larger payload. The 
first two show the results of the nonadaptive and adaptive controller starting with the nominal payload 
parameters (Figures 9.12 and 9.15). The nonadaptive controller represents the baseline controller. The 
last two sections show the two controllers starting with the smaller payload parameters (Figures 9.17 
and 9.18). 

9.3.1 Nonadaptive Controller with Nominal Payload Parameters 

This section presents the baseline nonadaptive controller utilizing the nominal set of payload parameters. 
The same back-and-forth slews are performed. Figure 9.12 shows the time-histories of the actual and 
desired trajectories. The plots indicate that even the baseline controller exhibits some overshoot 
characteristics. The adaptive control analysis in the next section shows that the spring forces 2 in the 
manipulators are not likely candidates for causing the overshoots. Actuator saturation, however, is a 
likely culprit, as Figures 9.13 and 9.14 show. The top right plot in Figure 9.13 shows that the right 
elbow torque barely reaches saturation during the second half of each slew. The base force 3 in the X 
direction also saturates during each slew. Actuator saturation prevents the robot from providing enough 
actuation to slow the payload sufficiently at the end of each slew and causes the trajectory overshoots. 


2 The spring effects are caused by electrical and pneumatic conduits inside the manipulators. 

3 The base forces and torques in the plots correspond to requested values from the controller. The actual forces and torques 
are supplied by the on-off thrusters, utilizing optimal bang-off-bang thruster mappings. 
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Figure 9.12: Large-Payload Trajectories — Baseline Nonadaptive Control 

The actual and desired payload trajectories, measured in the “port”-fixed reference frame, 
show the results for the baseline nonadaptive controller using the nominal values for payload 
parameters . The plots show that even the baseline controller has some overshoot. 
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Figure 9.14: Requested Base Forces and Torque — Baseline Nonadaptive Control 

These plots show the base forces and torques requested by the controller. The actual delivered 
forces and torques are derived from the oprimai bang-off bang thruster mappings , The base 
force in the X direction also saturates. The maximum torque capability is IN-m, so the base 
torque , as shown in the bottom plot , is still far from saturating. 
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9.3.2 Adaptive Controller with Nominal Payload Parameters 

This section shows once again that the adaptive controller, when starting with the nominal set of 
parameters, does not deteriorate the controller performance. In fact, the adaptive controller improves 
the orientation control, as Figure 9.15 shows. Figure 9.16 shows that the adaptive controller is 




Figure 9.15: Large- Payload Trajectories — Adaptive Control Starting with Nominal Valued for Pa- 
rameter Estimates 

The actual and desired payload trajectories, measured in the "port”-fixed reference frame, show 
the results for the basehne adaptive controller starting with the nominal payload parameters. 

The orientation-tracking performance seems to improve over that of the baseline controller by 
decreasing the overshoot. 


increasing the moment of inertia of the payload to decrease the orientation overshoot. That the 
moment-of-inertia estimate is monotonically increasing seems to rule out spring forces as the cause 
of the overshoots in the baseline controller. Spring forces will tend to aid the controller in certain 
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Figure 9.16: Large-Payload Parameter Estimates — Adaptive Control Starting with Nominal Values 
for Parameter Estimates 

The time histories of the payload parameter estimates show that the adaptive controller is 
increasing the moment-of-inertia estimate to improve the orientation-tracking performance. 


configurations, while hindering the controller in other configurations; this will be exhibited in the 
parameter estimate time-histories by increases in the estimate for one slew, then decreases for the slew 
in the opposite direction. This is not what the moment-of-inertia estimate in Figure 9.16 shows. 

Motor saturation, therefore, is the probable cause of the overshoots, and the adaptive controller 
compensates for it by increasing the inertia of the payload parameters estimates to improve the trajectory- 
tracking performance. 
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9.3.3 Nonadaptive Controller with Incorrect Payload Parameters 

The nonadaptive controller given the smaller payload parameters performs as poorly as the nonadaptive 
controller controlling the small payload using the larger payload parameters. Figure 9.17 shows the 
time-histories of the actual and desired payload position. This control is also unacceptable. 




Figure 9.17: Large-Payload Trajectories — Nonadaptive Control Starting with Incorrect Values for 
Parameter Estimates 

The actual and desired payload trajectories, measured in the “port” -fixed reference frame, show 
the results for the baseline nonadaptive controller actually controlling the larger payload , bur 
using the smaller-payload values for its parameters. 


9.3.4 Adaptive Controller with Incorrect Payload Parameters 

Enabling the adaptive control improves controller performance, as Figure 9.18 shows. The improve- 
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Figure 9.18: Large-Payload Trajectories — Adaptive Control Starting with Incorrect Values for Pa- 
rameter Estimates 

The actual and desired payload trajectories, measured in the “port” -fixed reference frame, 
show the results for the adaptive controller controlling the large payload , but starting with 
the smaller-payload values for its parameters. The plots show that although the performance 
improves over that of the nonadaptive controller, the improvement is not as rapid as that for 
the adaptive control for the small payload 


ment, however is not as dramatic as that of the adaptation for the smaller payload (Figures 9.9). The 
trajectory-tracking errors during the slews are not large enough to make the parameters converge quickly, 
as Figure 9.19 illustrates. These time-histories show that the parameters have not yet converged alter 
three slews. Higher adaptive update gains will improve the adaptation rate, but may deteriorate the 
performance when controlling the smaller payload; for example, a mass estimate change of 1kg represent 
10 percent of the larger payload mass, but represents 100 percent of the smaller payload mass. Thus a 





Location of C.M. (mm) Mass (kg) 


134 


Chapter 9. Implementation and Experimental Results 



Figure 9.19: Large-Payload Parameter Estimates — Adaptive Control Starting with Incorrect Values 
for Parameter Estimates 

These plots show the time histories of the parameter estimates when they are initially set to the 
parameters of the smaller payload. 


more rapid adaptation rate for the large payload may be too high for the small payload. 

Figure 9.20 shows the trajectory-tracking performance after the parameters have essentially con- 
verged. The plot shows that it is better than that of the baseline nonadaptive controller (see Figure 9. 12). 
Figure 9.21 presents the converged set of payload parameter estimates. They show that the estimated 
moment of inertia converges to higher values than the nominal values. The adaptive controller, there- 
fore, can improve the performance over that of the baseline controller by adjusting the parameters away 
from their nominal values. The adaptive controller may be compensating for some unmodelled effects 
to improve the trajectory-tracking errors. 
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Figure 9.20: Large-Payload Trajectories — Adaptive Control with Converged Parameter Estimates 

The actual and desired payload trajectories, measured in the “port” -fixed reference frame, show 
the results for the adaptive controller after the payload parameter estimates have converged. 

The x and y trajectory tracking performance is similar to the baseline nonadaptive controller 
(Figure 9.12), but the orientation tracking is better ; showing less overshoot. 


9.3.5 Large-Payload Control Summary 

Figure 9.22 compares the trajectory errors of the adaptive and nonadaptive controllers starting with the 
incorrect payload parameters. The time histories show that the adaptive controller performance is 
gradually improving over that of the nonadaptive controller. 

Figure 9.23 summarizes the overall performance for control of the large payload. It shows the 
trajectory-tracking errors of the baseline controller, the adaptive controller with converged parameters, 
and the nonadaptive controller utilizing the incorrect parameters. These plots demonstrate that the 
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Time (sec) Time (sec) 

Figure 9.21: Large-Payload Parameter Estimates — Adaptive Control with Converged Parameter 
Estimates 

These plots show the time histories of the parameter estimates after they have essentially 
converged l 


adaptive controller performed better than the baseline controller, after the parameters have converged. 
The nonadaptive controller using the small payload parameters is unacceptable for controlling the large 
payload. 
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Figure 9.22: Large-Payload Trajectory-Tracking Errors — Adaptive vs. Nonadaptive 

These plots show the time histories of the trajectory-tracking errors of adaptive and nonadaptive 
controllers starting with the incorrect payload parameter estimates. The adaptive controller 
shows gradual performance improvement over the nonadaptive controller. 
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Figure 9.23: Large-Payload Trajectory-Tracking Errors — Overall Performances 

These plots show the time histories of the trajectory-tracking errors of three controllers while 
controlling the large payload The adaptive controller — after the parameters have converged — 
performs slightly better than the baseline nonadapdve controller using the nominal payload 
parameters. The performance of the nonadapdve controller using the parameters for the smaller 
payload is unacceptable. 
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9.4 Adaptation to Manipulator Parameters 

This section presents results of adaptation to manipulator parameters utilizing the same to^-space 
adaptive control framework. The results show the effectiveness of the adaptive controller in reducing 
trajectory-tracking errors. Endpoint control is utilized for this experiment. The baseline controller is 
presented first with nominal arm parameters. The inertial parameters of the right arm are then set to 
zero and results of endpoint slews are shown without and with adaptation. 

9.4. 1 Nonadaptive Controller with Nominal Arm Parameters 

Figure 9.24 shows the endpoint trajectories of the baseline nonadaptive control using the nominal arm 
parameters. The steady-state offset are once again caused by the spring forces from wiring inside the 
manipulator. Integral control is disabled to show the performance of the baseline taskspzct controller 4 . 

9.4.2 Nonadaptive Controller with Incorrect Arm Parameters 

Setting to zero the estimates for the inertial parameters of the right arm — mi, m 2 , / 1 , and I 2 — 
effectively disables the inverse-dynamics feed-forward portion of the task-s pace controller. Figure 9.25 
shows the controller performance. The trajectory- following in the X direction is quite good, but there 
is noticeable overshoot in the Y direction. The bottom “X vs. Y” plot shows more clearly the path of 
the endpoint and the deterioration in performance. 


integral control is enabled during actual operation to enable the capture of the free-flying object. 
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Figure 9.24: Endpoint Position — Baseline Nonadaptive Control 

These plots show the actual and desired endpoint trajectories in inertial space of the baseline 
nonadaptive controller using nominal arm parameters. 77ie bottom plot shows the X vs. Y 
plot , representing an “ overhead " view of the path traced out by the endpoint in inertial space. 
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Figure 9.25: Endpoint Position — Nonadaptive Control Starting with Incorrect Values for Parameter 
Estimates 

These plots show the actual and desired endpoint trajectories in inertial space of the nonadaptive 
controller using zero as estimates for the arm inertial parameters. 
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9.4.3 Adaptive Controller with Incorrect Ann Parameters 

Enabling adaptive control improves the controller performance, as Figure 9.26 illustrates. Comparing 
with the baseline controller performance of Figure 9.24 shows that adaptive control performance is as 
good as the baseline controller by the third slew. Figure 9.27, however, shows that the parameters 





Figure 9.26: Endpoint Position — Adaptive Control Starting with Incorrect Values for Parameter 
Estimates 

These plots show the actual and desired endpoint trajectories in inertial space of the adaptive 
controller starting with zero as estimates for the arm inertial parameters . 

did not converge toward their nominal values. The PD controller did reasonably well in tracking the 
back-and-forth endpoint slews, so there was not enough tracking errors for the adaptive controller to 
allow good parameter convergence. Endpoint motions with more excitation is necessary to make the 
parameters converge. Nevertheless, the task-s pace adaptive controller provides good trajectory-tracking 
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performance. 

9.5 Summary 

The new task-s pace adaptive controller can provide both payload adaptation and manipulator adaptation 
to improve trajectory following. For the payload, where there are few parameters, the simple trajectories 
provide sufficient excitation to allow identification of the payload parameters to converge close to their 
correct values. In addition, an “incorrect” set of parameters can actually improve the trajectory-tracking 
performance by compensating for unmodelled effects 5 . The adaptive controller also improves endpoint- 
tracking errors; but there is not enough excitation to make identification of all the parameters containing 
the arm parameters converge. 


5 Uhlik [42] demonstrated a different aspect of this by showing that the use of an “incorrect” set of parameters for the robot 
manipulator can improve the identification of the payload; but that these “incorrect” manipulator parameters cannot, however, 
be used for stable high-performance control. 
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Figure 9.27: Parameter Estimates — Adaptive Control Starting with Incorrect Values for Parameter 
Estimates 

These plots show the time histories of the parameters for the adaptive controller when m\ , mi, 

I\, and Ii are initial// set to zero. There is obviously not enough excitation in the trajectory to 
effect parameter convergence . 



Chapter 10 


Conclusions 


This chapter summarizes the results of this research and draws some conclusions. It also describes some 
continuing research, and suggests possible future extensions. 

10.1 Summary 

This dissertation constitutes the theoretical development of a new general adaptive control framework 
and the experimental demonstration of the effectiveness of the new concept. This research showed that 
the resulting new adaptive controller maintains good stability, without sacrificing performance, in the 
presence of unknown or changing parameters. 

The new task-s pace adaptive control framework affords the free-flying space robot — a complex 
system containing multiple, interacting manipulators — effective adaptive control in all control modes. 
The adaptive control readily handles joint-level control in the same manner as cooperative object control, 
needing only the appropriate Jacobian and torque mapping for the control mode. 

The task-space adaptive control is a general algorithm for systems with rigid members. The 
theoretical development does not restrict the system to be just a free-flying space robot with two 
manipulators operating in two dimensions: A system of any number of fixed or free-flying robots 
with any number of rigid, nonredundant manipulators in three-dimensions may take advantage of this 
adaptive controller. 

The system concatenation concept for control yields efficient, incremental generation of system models 
for multiple, interacting systems. New manipulators added to the system are typically represented in 
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the system models as additional block-diagonal matrices. The models existing before the addition are 
unaffected. The block-diagonal system matrices also lend themselves well to parallel computing. 

System concatenation also keeps the adaptable parameters of each subsystem separated when in- 
corporated into the task-s pace adaptive algorithm. This allows operators to maximize their intuition 
and maximize the utility of a priori knowledge of the controlled system. Applying heavier adaptation 
weighting on the parameters of a subsystem that are poorly known ensures a quicker convergence of the 
adaptive algorithm. The separable parameters furnish an elegant method of weighting the parameter 
subspaces for faster convergence without the need for swapping adaptive controllers. 

The modularity of the task-spzce adaptive controller means easy implementation and multiple- 
control-mode support with minimal effort. The basic controller blocks and adaptive update modules 
do not change when switching control modes. Only the Jacobian and torque mapping modules, which 
typically involve easily derived kinematic relationships, need to change. Since the vector of adaptable 
parameters also does not change with control modes, there are no "glitches” during control mode 
switches. 

The experimental results show that the new adaptive control achieves robustness toward plant 
changes with little cost to performance. In fact, the trajectory-following plots indicate that the adaptive 
controller performs better than the nominal controller, illustrating that the new adaptive control can 
compensate strongly for the effects of mismodelled and unmodelled parts of the system 1 . 

The system concatenation and task - space concepts, even without adaptation, comprise an elegant 
formulation of control of complex systems. They formulate multiple-manipulator control as a complete 
system in a more "traditional” manner. This allows the well-known computed-torque or inverse- 
dynamics analyses to be performed direedy, without separating object control from manipulator control 
into two distinct steps. 

Perhaps the most significant contribution of the r^-space adaptive control framework is that it is 
a framework. A particular joint-space adaptive controller was extended to provide the adaptive portion 
of the framework; but the framework does not require that particular adaptive scheme. Other adaptive 
controllers initially designed for a single-manipulator robot may take advantage of this framework to 
be extended to multiple, cooperative-manipulator control. 


l The laboratory’s vision systems, which the author built early on, were essential to the success of these experiments. 
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10.2 Continuing Research 

Ongoing research at the Aerospace Robotics Laboratory (ARL) and Information Sciences Laboratory 
(ISL) of Stanford University naturally complements the work presented in this thesis. One area is the 
use of recursive algorithms to achieve even more efficient controller implementations. Another is in the 
control of multiple, cooperating manipulators possessing kinematic redundancy. 

The tout-space adaptive control framework makes extensive use of Jacobian-like matrices to effect 
control in the task space. The algorithm requires both the Jacobian and its matrix inverse. As the system 
complexity grows, these computations may become prohibitively expensive. Recursive implementation 
of the inverse Jacobian have shown that computations can be gready reduced even for a moderately 
complex system. 

Additionally, the system concatenation concept can be carried to the limit, where each link of 
a manipulator represents a subsystem. At this extreme, the inverse dynamics control can also be 
implemented recursively. Coupled with recursive Jacobian implementation, these order - N algorithms 
provide very computationally efficient controllers. Research is progressing on extending these algorithms 
to the control of multiple coopcrating-manipulator robots. 

A redundant manipulator possesses more degrees of freedom than are needed for definitive control. 
Additional degrees of freedom afford the manipulator the ability to maneuver around obstacles, to 
save fuel, and to avoid kinematic singularity, without affecting the primary control objectives. As one 
example of its usefulness, this ability allows a manipulator to work in a cluttered environment with 
relative ease. The challenge in controlling a redundant manipulator involves developing an endpoint 
controller that will “naturally” avoid obstacles and singularities without operator intervention. Many 
researchers have already developed redundant controllers for single manipulators. Ongoing research at 
ARL is planned to formulate control for multiple, cooperating, redundant manipulators, but to do so 
in a manner that is easily extensible as more manipulators are added. 

ARL is also collaborating with the Stanford Computer Science Robotics Laboratory (CSRL) to merge 
sophisticated path-planning algorithms with real-world robotic systems. The initial experiments are 
performed utilizing fixed-base robots, and the algorithms will be transferred to the Multiple-Manipulator 
Space Robot facility in the near future. 
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10.3 Suggestions for Future Research 

An obvious area of further research is to extend both the recursive algorithms and the multiple redundant- 
manipulator control work to the realm of adaptive control. The combination of task - space adaptive 
control, recursive algorithms, and redundant-manipulator control capabilities will form a very powerful 
and useful control framework. 

The task-s pace adaptive control has been developed for manipulator systems composed of connected 
rigid bodies. The applicability to systems possessing flexibility must be examined. Two types of flexibility 
can exist in robots: drive flexibility and flexibility distributed along the links of a manipulator. The 
former is quite common in industrial robots, and both are important in the Space Shuttle Remote 
Manipulator System (RMS); the latter is expected to be a concern in space-based manipulators where 
increased link flexibility is traded off for weight reduction. Since flexibility can severely limit manipulator 
performance, controlling well when it is present is important. Major pioneering experimental research 
has already been completed at ARL on the quick precise control of very flexible manipulator arms and 
on manipulators with flexible joint-drive systems [ 36 , 31 , 42 ]. 

Although the task-spucc adaptive control does not yet expressly address distributed flexibility, its 
applicability is high for robots with joint flexibility. The successful application already completed of 
local joint-torque control in cooperating manipulators with joint flexibility makes one quite optimistic 
about the direct application of task - space adaptive control: The joint-torque inner-loop control makes 
the actuators behave as perfect torque sources, hiding the flexibility from the adaptive controller. 
Experimentation, of course, must be conducted to determine whether further extensions must be made 
to handle joint flexibility. Still further research is needed to determine the applicability of the task-s pace 
adaptive control framework for distributed flexibility. 

This dissertation demonstrates the task-space adaptive control framework with a specific adaptive 
update algorithm. But since this framework does not limit the choice of the adaptive controller, 
further valuable study can be made to compare and contrast the performance of other adaptive schemes, 
including exponentially forgetting least squares (EFLS) algorithms that take advantage of past data 
histories. To illustrate this, the current implementation has difficulties identifying the spring forces 
caused by tubing and wiring in our laboratory space-robot manipulators. The EFLS algorithms may be 
able to improve the parameter convergence for such terms. 

Although the experimental results presented in this thesis indicate that the adaptive controller is 
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capable of improving the performance by overcoming effects of unmodelled dynamics, further study 
needs to be done to classify the explicit classes of unmodelled effects in which the task-s pace adaptive 
control remains effective. 

The fcw£-space adaptive controller can effectively adapt to an unknown payload, but only after 
the robot has acquired it. The capture algorithm in the current hierarchical controller plots a straight 
intercept trajectory, based on the objects position and velocity. This strategy is valid for objects 
significandy less massive than the robot, but may be fatal for very massive objects. Since the adaptive 
controller will not get a chance to determine which is the case before the capture, more sophisticated 
capture algorithms need to be developed. The intercept trajectory, for example, should parallel that of 
the object — matching both position and velocity — to minimize the danger while capturing the object. 
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Supporting Calculations for Lyapunov 
Proof 


A. 1 Proof of Identities for M d 


The definition of Mp is repeated here: 


t vyf ^ \ & v*' ^M(q) ^ 

Mg(q,y) = 2^ ^ ye«- 

tl d * 


where e* G lR n is the zth unit vector. 
Identity 1: 


Proof : 


Identity 2\ 


d (M(q)y) 

dq 


= Mi)(q, y) 


g(M(q)y) _ f9M(q)y T 


dq 
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Proof : 


Identity 3 : 
Proof : 
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(A- 1) 


(A.4) 


A.2 Representation for C 


The representation for the matrix of Coriolis and centrifugal terms, C(q, q), in the equations of motion 
for a robot with rigid links is not unique, although the vector, C(q, q)q, is uniquely specified. The 
stability proof found in this thesis utilizes a particular representation: 

C(q» q) = Mi?(q, q) - ^M£(q, q) (A.5) 


The following shows that this is a valid choice: 


C(q, q)q 


(A.5) 


(A.2.A.3) 



q 



q 
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( 3 . 15 ) 


c(q>q)q 


Please note, however, that: 

(*Mq, q) - ^Mp(q, q)) * ^M(q, q) - \ j (A.6) 

This representation for C(q, q) also does not satisfy the skew-symmetry property needed in the sliding- 
mode adaptive controller [37]: 


y T (M(q,q)-2C(q,q))y = 0 V y € IR n (A.7) 

This property is not required for the Lyapunov proof in this dissertation. Additionally, note that this 
representation for C(q, q), Equation (A.5), is useful only in the stability proof. It is not needed for 
implementation of the control or adaptive update laws, which use the combination, C(q, q)q. 


A. 2.1 Example 

To illustrate the different representations, use once again the planar two-link arm example in Figure 2. 1 . 
The choice of C(q, q) utilized in Equation (2.5) is: 


C(q, q) 


-m 2 l\l$ sin(g 2 )g 2 -m 2 l x l$ sin(g 2 ) (?i + g 2 ) 
m 2 l\l 2 sin(q 2 )<ji 0 


which satisfies the skew-symmetric property. 

Using the representation defined by Equation (A.5) yields: 


C(q, q) 


0 -m 2 l x l 2 sin(g 2 ) (2^ + q 2 ) 

m 2 l x ll sin(g 2 ) (qj - \q 2 ) \m 2 lil 2 sin (q 2 )q 2 


Using either representation for C(q, q), the following is true: 


(A.8) 


(A.9) 


c(q, q)q = 


—m 2 l\l 2 sin(g 2 ) (2gi + q 2 ) q 2 
m 2 l\l 2 sin (q 2 )q x 


(A. 10) 


showing that C(q, q)q is uniquely specified. 
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A.3 Control and Adaptive Law Transformations 

This section shows the equivalency of the control and adaptation laws developed for equations of motion 
written in terms of q s and those written in terms of generalized speeds. 

The the control and adaptation laws derived for equations of motion in terms of generalized speeds 
is repeated here as: 

F = M'(q)u <i + C , (q,u < ,)u < , + G / (q) + KVu + W- r Kpq (A.11) 

( 4 . 4 ) 

0 rY' T (q, u d , u d , u d ) (u + cW T q) (A.12) 

where 

F = W" T t (A. 13) 

( 4 . 3 ) 

A.3. 1 Control Law Equivalence 

Given the transformation equations derived in Chapter 4, 

F = W ~ t t 
M'(q) = W- T M(q)W-‘ 

C'(q,u) = W^CfoW-^W- 1 (A. 14) 

G' = W- T G(q) 

Y'(q, u, u, u)0 = W -T Y(q, W -l u, W _1 u, W -1 u)0 

substitute them into Equation (A.1 1): 

W -T r = W- r M(q)W- , u <J + W~ T C(q, W^u^W" 1 ^ + W _T G(q) 
+W" t W t KVu + W~ T K P q 

= W -T (M(q)q d + C(q, q d )q d + G(q) + W T KVw 4 + K P q) (A. 15) 
Multiply both sides of Equation (A. 15) by W r yields the Bayard and Wen control law, 

T = M(q)q d + C(q, q d )q d + G(q) + W T K^Wq + K P q 

( 3 . 4 ) 

with a slightly different velocity gain matrix, Ky = W t KVW. 
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A.3.2 Adaptation Law Equivalence 

Stricdy speaking, the adaptation law in Equation (A. 12) is derived from the following condition: 

+ YT ^ Ud ’ Ud ’ + cW ^ (A. 16) 

Substituting the transformation equations into Equation (A. 16) gives: 

0 = 0 T r~ l 9 + 6> r Y r (q, W _1 Urf, W -1 u<j, W _1 u,j)W _1 (u + cWq) 

(4 = 8) 0 T r~ l e + 0 T Y T (ci, q d , q d , q d ) (4 + cq) 

=> 0 - ~rY T {q,q d ,q d ,q d ) (q + cq) 

=> 0 = rY T (q, q d , q d , q d ) (q + cq) (A. 17) 

since 9 = — 9, Equation (A. 17) is the Bayard and Wen adaptive update law. 

This proves that the two sets of control and adaptive update laws are equivalent. 

A.4 Torques to Endpoint Forces using Virtual Work 

This section shows, using virtual work arguments, the familiar expression relating endpoint forces and 
actuator torques: 

r = J r (q)F tip (A. 18) 

The work done by the manipulator actuators as they move through a virtual displacements at each 
joint, £q, is: 

6W = r T 6q (A. 19) 


The work delivered at the endpoint of the manipulator through a virtual displacement at the endpoint, 
Sx, is: 



6W = F tipT 6x 

(A. 20) 

These must be equal, hence: 

t t 6q = F tipT 6x 

(A.19A20) 

(A.21) 

As the virtual displacements go 

to zero, 



c dx 

6x = — <5q 

(A. 22) 


= J(q)<5q 

(A.23) 
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Substituting Equation (A.23) into Equation (A.21) for 6x gives: 

(A - 24) 

Transposing both sides of Equation (A.24) gives: 

6q T T = 6q T J T (q)F t,p 

(A.24) 

=► r = J r (q)F tip (A.25) 


Thus, showing the relationship in Equation (A. 18). 
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Point Grabber II Vision System 


This appendix includes the User’s Manual for the Point Grabber II Vision system, resolution-testing 
plots, the schematics, and the PALASM listings of the logic for the PALs (Programmable Array Logic) 
used in the Point Grabber II board. 

B.l User’s Manual 

The user’s manual start on the following page. 
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PointGrabber II 
User’s Manual 
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Introduction 


Point Grabber II is a specialized single-board VMEbus-based vision processing unit that provides real- 
time systems with high speed, high resolution vision information from CCD cameras. It is ideally suited 
for applications in a robotics and control environment where high speed sampling is essential to achieve 
high performance. 

Coupled with non-interlaced CCD television cameras and RTFs Visionserver software. Point Grabber n 
can provide frame updates at 60 Hz with resolutions better than 1/40 of a pixel. Over a field of view of 2 
meters square, that translates to a resolution of 0.2 mm square. 

To decrease the computational burden on host computers. Point Grabber II locates, digitizes, and stores 
only bright points in the field of view. Bright markers, such as LEDs, placed on objects of interest 
allows appropriate software to determine the position and orientation of the objects from Point Grabber II 
data. Unlike “Frame Grabbers”, Point Grabber II provides data that all correspond directly to markers, 
eliminating the need for host computers to perform sophisticated scene analysis computations. 
Additionally, each Point Grabber II board can process data from two cameras simultaneously. 


System Requirements 


VMEbus system 
Host CPU card 

Non-interlaced CCD camera capable of accepting external horizontal and vertical 
synchronization signals 
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General Description 


Point Grabber II detects, registers, and digitizes brightly illuminated targets from video signals generated 
by up to two cameras. Either active infrared LEDs or passive high reflectance stickers reflecting light 
from an incandescent source can provide the bright targets . Visible-cut, infrared-pass filters placed over 
the camera lenses will reject ambient visible light . Independent programmable threshold voltages for 
each camera video input determine the transition between bright and dark. The vision board “detects” a 
bright target when the video input signal from either camera rises above its respective threshold voltage. 

Point Grabber II tracks the current horizontal (X) position and vertical (Y) position of the video scan of 
each camera. The video board can, therefore, register the position, measured in pixels, of the each bright 
image. The X position is measured starting from the left-hand side of the field of view, and the Y 
position is measured starting from the top of the field of view. By sending horizontal and vertical 
synchronization signals (HSYNC and VSYNC, respectively) to the CCD cameras, and by utilizing the 
phase-locked loops (PLL) in the cameras. Point Grabber II provides very stable tracking of the video 
scan. This method also ensures that, if two cameras are used, the video scans of both cameras are in 
strict synchronization. 

Besides registering the position of bright pixels, 8-bit analog to digital (A/D) converters on the vision 
board digitize the video signals corresponding to those pixels. The board stores the digitized values with 
the position information. Vision processing software may use the digitized values to achieve the 1/40 
pixel resolution. The dynamic range of Point Grabber II is user programmable. Point Grabber II 
automatically sets the lower reference voltage of each A/D converter to the corresponding threshold 
voltage for each camera. By programming the upper reference voltage to be the highest expected video 
input voltage, one can maximize sensitivity. 

Point Grabber II stores the vision information in four (4) First-In-First-Out (FIFO) registers. One FIFO 
(X FIFO) records the X position, one (Y FIFO) records the Y position, and two (Z0 FIFO and Z1 FIFO) 
record the digitized values representing pixel brightness as seen by each camera. The vision board keeps 
all data in the FIFOs in strict synchronization. That is, for each bright pixel detected by one or both 
cameras, Point Grabber II writes data to all FIFOs. If only one camera detects a bright pixel, the 
digitized value stored for the other camera is zero. Additionally, the vision board writes an extra data set 
to the FIFOs at the end of each video frame. This data set, or frame marker, allows Point Grabber II to 
continue storing data for the next video frame while vision processing software is still reading data for 
the current frame. The implication of Point Grabber IPs storage scheme is that the vision processing 
software also must perform FIFO reads in strict synchronization. That is, the FIFOs must be accessed an 
equal number of times. In the event that synchronization is lost, the software may reset the FIFOs via a 
hardware register. 

Point Grabber II functions as a VMEbus interrupter. With interrupts enabled, the vision board generates 
interrupts at the end of every camera video frame — typically every 1/60 of a second. The interrupt level 
and interrupt vector are user programmable. 

Additional registers on Point Grabber II provide separate camera enables, empty/full status of each FIFO, 
and board-level reset. Refer to the appropriate sections for more detailed descriptions of the location and 
usage of the registers mentioned in this section. 
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Installation 

Address Jumpers 

Point Grabber II can be configured to reside in any 256-byte segment in short I/O space, starting at hex 
address OxFFFFOOOO and ending at OxFFFFFFFF. The address jumpers, J1 through J8, are located near 
the PI connector. The jumpers represent bits 8 through 15 of the board address. With a jumper installed, 
the corresponding bit is set to 0. The default configuration, with only J1 installed, makes the board 
appear at the 256-byte segment starting at OxFFFFFEOO. 



Bit value 

installed (x) 

0 


1 


Jumper 

J8 

J7 

J6 

J5 

J4 

J3 

J2 

J1 

Address Bit 

15 

14 

13 

12 

11 

10 

9 

8 

Default setting 

o 

0 

o 

0 

0 

0 

0 

X 


Camera Synchronization Timing 

No further timing configuration is required to support the Pulnix TM-440S camera. 

Point Grabber II provides precise synchronization with its cameras by utilizing the phase-locked-loop 
(PLL) circuitry found in many CCD cameras. The PLL locks on to the user configurable horizontal and 
vertical synchronization signals supplied by Point Grabber II. 

The camera specifications must be available before the synchronization timing can be configured. The 
standard crystal oscillator (U17) provided with Point Grabber II has a frequency of 14.318 MHz. This 
value must be twice the frequency of the pixel clock of the camera. From the camera specifications, 
make a note of the required number of pixel clocks per horizontal line (horizontal count) and the number 
horizontal lines per field (vertical count) produced in the non-interlaced mode. These numbers are 
essential to the proper configuration of the vision board. 

Use the six 4-position dip switches, SI through S6, to configure the camera synchronization timing. The 
left group of three switches (SI through S3) represents the horizontal count, while the right group (S4 
through S6) represents the vertical count. Letting the ON position represent logical 1, and letting each 
group of dip switches represent a 12-bit binary number, set the switches for the desired horizontal count 
minus one and vertical count minus one, respectively. 

For example, the default settings on Point Grabber II are for the Pulnix TM-440S camera, which requires 
a horizontal count of 455 and a vertical count of 262. The resulting switches settings are: 


Switch 

SI 

S2 

S3 

S4 

S5 

S6 | 

Setting (l=ON, 0=OFF) 

0001 

1100 

0110 




Decimal value 

454 

261 l 
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Register Descriptions 

The Point Grabber II address space occupies a 256-byte segment selected by the address jumpers (see the 
Installation section). The default segment starts at hex address OxFFFFFEOO. The following register 
descriptions will give the addresses of the registers in terms of an offset into the selected 256-byte 
segment. Addresses not mentioned are unused. 

FIFO Registers 

Point Grabber II stores its vision image information in four (4) First-In-First-Out (FIFO) registers. Each 
FIFO is 9 bits wide and 512 deep. To the VMEbus, these FIFOs are read-only, 16-bit word registers. 

The vision board stores the X and Y location of each bright pixel in the X and Y FIFOs, respectively. It 
stores the digitized value of a bright pixel from the first camera (Camera 0) in the Z0 FIFO, and that of 
the second camera (Camera 1) in the Z1 FIFO. Since all the FIFOs are written when a bright pixel is 
detected in either or both cameras, the lower 8 bits of the Z0 and Z1 FIFOs will both be nonzero only if 
both cameras detect bright targets simultaneously. If only one camera detects a bright target, the data in 
the other Z FIFO will be zero. Any vision processing software must perform an equal number of reads 
from each FIFO to ensure that the data from all FIFOs on each set of reads refer to the same event. 

At the end of each video frame. Point Grabber II additionally stores a data set, called the frame marker, 
into the FIFOs. By monitoring the FIFO data for the frame marker, vision processing software can easily 
determine the end of a video frame, while allowing the vision board to continue storing vision data for 
the next video frame. This scheme takes advantage of the “double-buffering” capability of the FIFOs, 
and ensures that no vision information is lost while performing vision processing. 

Because the FIFOs have finite depth, it is possible for them to become full. This will happen if too many 
bright targets appear in the field of view, which may be caused by excessive illumination or inappropriate 
threshold voltages. Similarly, it is possible for the FIFOs to be empty, which indicates that no data is 
currently available. When used as a bus interrupter, empty FIFOs also indicate an error, since they 
should contain at least a frame marker data set. Either of these conditions indicate error, and vision 
processing software should check the status register (see below) before proceeding to read FIFO data. 

The recommended action for empty/full error is to reset the FIFOs (see below). 


X FIFO (R) (OxDO) 


Bits 15-9 

Bits 8-0 

Unused 

X position + (511- horizontal count) 


The X FIFO is a read-only register located at offset OxDO. It should be accessed via 16-bit word reads, 
although only the lower 9 bits are significant. Each value obtained from the X FIFO is offset from the 
true X field-of-view position by the quantity (51 1 - horizontal count), where “horizontal count” is the 
total number of pixels per horizontal scan line. Consult the camera specifications for the correct 
horizontal count value. 


Y FIFO (R) (0xD2) 


Bits 15-9 

Bits 8-0 

Unused 

Y position + (511 - vertical count) 


The Y FIFO is a read-only register located at offset 0xD2. It should be accessed via 16-bit word reads, 
although only the lower 9 bits are significant. Each value obtained from the Y FIFO is offset from the 
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true Y field-of-view position by the quantity (51 1 - vertical count), where “vertical count” is the total 
number of horizontal scan lines. Consult the camera specifications for the correct vertical count value. 


Performing the above offset calculation places the coordinate (1, 1) at the top left comer of the field of 
view, as viewed on a television monitor. The coordinate (horizontal count - 1, vertical count - 1) is at the 
lower right comer. This is a left-handed coordinate system, and vision processing software will need to 
take this into account. 


ZO FIFO (R) (0xD4) 


Bits 15-9 

Bit 8 

Bits 7-0 

Unused 

Frame marker 

Camera 0 pixel value 


The ZO FIFO is a read-only register located at offset 0xD4. It should be accessed via 16-bit word reads, 
although only the lower 9 bits are significant. The ZO FIFO stores the digitized values of each bright 
pixel detected by the first camera (Camera 0) in the lower 8 bits. The ninth bit (Bit 8) is a frame marker. 
Point Grabber II sets Bit 8 of the ZO FIFO at the end of each video frame. Because of the way most CCD 
cameras operate, the lower 8 bits of the ZO FIFO will be zero when the ninth bit is 1. 


Z1 FIFO (R) (0xD6) 


Bits 15-9 

Bit 8 

Bits 7-0 

Unused 

Frame marker 

Camera 1 pixel value 


The Z1 FIFO is a read-only register located at offset 0xD6. It should be accessed via 16-bit word reads, 
although only the lower 9 bits are significant. The Z1 FIFO stores the digitized values of each bright 
pixel detected by the second camera (Camera 1) in the lower 8 bits. The ninth bit (Bit 8) is a frame 
marker. Point Grabber II sets Bit 8 of the Z1 FIFO at the end of each video frame. Because of the way 
most CCD cameras operate, the lower 8 bits of the ZO FIFO will be zero when the ninth bit is 1 . The Z1 
FIFO frame marker serves the same purpose as the ZO FIFO frame marker. Both are provided to allow 
software synchronization checks. 


Status Register 


Status Register (R) (0xD9) 


Bit 7 

Bit 6 

Bit 5 

Bit 4 

Bit 3 

Bit 2 

Bit 1 

BitO 

Z1 Full 


Y Full 

X Full 






The Status Register is a read-only byte register located at offset 0xD9. Each bit in the register indicates 
the full or empty status of each of the four FIFOs. Software should consult this register before 
performing any FIFO reads; any nonzero bit indicates an error. If it detects an error, the software should 
reset the FIFOs via the FIFO Reset Register (see below) to resynchronize the FIFOs. 
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Threshold Registers 

Threshold 0 Register (W) (OxDl) 
Bits 7-0 

Camera 0 threshold 


The Threshold 0 Register is a write-only byte register located at offset OxDl. The 8-bit value represents 
a threshold voltage, from 0 V to 2 V, for Camera 0. The threshold voltage indicates the transition of the 
video signal from dark to bright. When the Camera 0 video signal voltage is higher than the threshold, 
Point Grabber II stores the X and Y scan location as well as the digitized signal strength. The threshold 
voltage is also the lower reference voltage of the corresponding A/D converter. 

Threshold 1 Register (W) (OxDS) 

Bits 7-0 

Camera 1 threshold 


The Threshold 1 Register is a write-only byte register located at offset 0xD5. The 8-bit value represents 
a threshold voltage, from 0 V to 2 V, for Camera 1 . The threshold voltage indicates the transition of the 
video signal from dark to bright. When the Camera 1 video signal voltage is higher than the threshold, 
Point Grabber II stores the X and Y scan location as well as the digitized signal strength. The threshold 
voltage is also the lower reference voltage of the corresponding A/D converter. 

ADC Limit Registers 

ADC 0 Limit Register (W) (0xD3) 

Bits 7-0 

Camera 0 ADC limit 


The ADC 0 Limit Register is a write-only byte register located at offset 0xD3. The 8-bit value represents 
the upper reference voltage, from 0 V to 2 V, for the Camera 0 A/D converter. The setting of the upper 
reference voltage alters the sensitivity of Point Grabber II. By setting ADC 0 Limit to correspond to the 
highest expected voltage of the Camera 0 video signal, one can maximize sensitivity. 

ADC 1 Limit Register (W) (0xD7) 

Bits 7-0 

Camera 1 ADC limit 


The ADC 1 Limit Register is a write-only byte register located at offset 0xD7. The 8-bit value represents 
the upper reference voltage, from 0 V to 2 V, for the Camera 1 A/D converter. The setting of the upper 
reference voltage alters the sensitivity of Point Grabber II. By setting ADC 1 Limit to correspond to the 
highest expected voltage of the Camera 1 video signal, one can maximize sensitivity. 
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Enable Register 

Enable Register (R/W) (OxDB) 


Bit 7 

Bits 6-2 

Bit 1 

BitO 

Interrupt Enable 

Unused 

Camera 1 Enable 

Camera 0 Enable 


The Enable Register is a read-write byte register located at offset OxDB. Logical 1 indicates enable. One 
can individually control data collection from each camera via the corresponding enable bits. When a 
camera is “disabled”, only the data storage for that camera is disabled. The camera is still operational, 
and the video signal can still be viewed on a television monitor. Interrupt generation also may be 
enabled or disabled using bit 7 of the Enable Register. Bits 6 through 2 are unused. 

FIFO Reset Register 

FIFO Reset Register (W) (OxDD) 

Bits 7-0 

Don't Care 


The FIFO Reset Register is a write-only byte register located at offset OxDD. Any writes to this address 
location will reset and empty all FIFOs. The value written is irrelevant. 

The FIFOs need to be reset immediately after enabling interrupts to ensure that the data will be 
synchronized. They should also be reset after an error has been detected. 

Board Reset Register 

Board Reset Register (W) (OxDF) 

___________ Bits7^0____ < 

Don’t Care 


The Board Reset Register is a write-only byte register located at offset OxDF. Any writes to this address 
location will reset Point Grabber II. The value written is irrelevant. Resetting the board brings Point 
Grabber II to the power-on condition, which means that the cameras are disabled, the FIFOs are reset, the 
threshold and ADC limit voltages are zero, interrupt is disabled, and the interrupt level and vector are 
zero. 


Interrupt Control Register 


Interrupt Control Register (R/W) (OxEl) 


Bit 7 

Bit 6 

Bit5 

Bit 4 

Bit 3 

Bit 2 

Bit 1 

BitO 

F 

FAC 

XI IN* 

IRE 

IRAC 

L2 

LI 

L0 


The Interrupt Control Register is a read-write byte register located at offset OxEl. This register controls 
the interrupt level, a separate interrupt enable, and fields that determine actions during an interrupt 
acknowledge cycle. The fields are defined as follows: 
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1. Interrupt level (L2, LI, LO) — The least significant 3-bit field of the register 
determines the level at which an interrupt will be generated: 


L2 

LI 

LO 

IRQ Level 

0 

0 

0 

Disabled 

0 

0 

1 

IRQ1 

0 

1 

0 

IRQ2 

0 

1 

1 

IRQ3 

1 

0 

0 

IRQ4 

1 

0 

1 

IRQ5 

1 

1 

0 

IRQ6 

1 

1 

1 

IRQ7 


A value of zero in the field disables the interrupt. 

2. Interrupt Enable (IRE) — This field (Bit 4) must be set (high level) to enable the bus 
interrupt request. 

3. Interrupt Auto-Clear (IRAC) — If the IRAC is set (Bit 3), IRE (Bit 4) is cleared during 
an interrupt acknowledge cycle responding to this request. This action of clearing IRE 
disables further interrupt request. To re-enable the interrupt associated with this 
register, IRE must be set again by writing to the control register. Leave this bit unset 
during normal operations. 

4. Extemal/Intemal (X/IN*) — Always clear (low level) this bit for Point Grabber II. 

5. Flag (F) — Bit 7 is a flag that can be used in conjunction with the test and set 
instruction of the MC680xx family of microprocessors. It can be changed without 
affecting Point Grabber II operation. This flag may be useful for processor-to-processor 
communication and resource allocation (i.e., symaphors). This flag is typically not 
used. 

6. Flag Auto-Clear (FAC) — If FAC (Bit 6) is set, the Flag bit is automatically cleared 
during an interrupt acknowledge cycle. 


Interrupt Vector Register 

Interrupt Vector Register (R/W) (0xE3) 

Bits 7-0 

Interrupt Vector 


The Interrupt Vector Register is a read-write byte register located at offset 0xE3. This 8-bit interrupt 
vector is supplied, during an interrupt acknowledge cycle, to the CPU for calculating the location of the 
interrupt service routine. For the VMEbus, the routine is located at an address that is four (4) times the 
vector value. 
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B.2 Resolution Tests 

Figures B.2 and B.3 show the steady-state noise characteristics of the Point Grabber II vision system. 
The data was taken from the global vision system viewing a single LED. The vision system covers a 
field of view of 2.4m in the x direction, and 1.7m in y direction. The noise is about .2mm in the x 
direction and about .1mm in the y direction — a resolution of better than 1 in 10,000. 



Time (sec) 

Figure B.2: Noise Characteristics — X Direction 


Figures B.4 and B.5 show the resolution of the vision system by demonstrating the transient 
behavior. The LED, mounted on one of the free-flying payload objects, is moved slowly and smoothly 
along a diagonal. The plots show relatively smooth, monotonic functions of time for the LED x and 
y positions. Although there are no sharp jumps at camera-pixel boundaries, the pixel effect can still be 
seen as deviations from a straight line in the plots. The “bumps” in the x direction occur at about every 
5mm, and those in the y direction occur at about every 8mm. 

There are 380 active camera pixels in the x direction and 190 active pixels in the y direction. The 
size of each pixel, therefore, is calculated to be: 

x slze = 2400mm/380 = 6.6mm 
Vsize = 1700mm/ 190 = 8.9mm 
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Time (sec) 

Figure B.3: Noise Characteristics — Y Direction 

This agrees reasonably well with the observed behavior in Figures B.4 and B.5. Larger targets or LED s 
that cover more camera pixels will alleviate this effect. 
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B.3 Schematics 

The schematics start on the following page. 
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B.4 

PALASM Listings 

B.4.1 

DTACK Timing 

TITLE 

PG DTACK TIMING 

PATTERN 1 

REVISION 3 . OB 

AUTHOR 

VINCENT CHEN 

COMPANY ARL, STANFORD UNIVERSITY 

DATE 

09-30-90 


This Pal generates several handshaking signals required for the interface 
between the Point Grabber and the VME Bus: 

/VRESET is the VME reset signal, synchronized to SYSCLK. 

/ALATCH is the address latch signal. BSSTROBE initiates the latch. 

The latch is terminated by DS and AS going invalid during a READ cycle 
(ENDC) indicating that the VME Bus has completed the access, so we no longer 
need to address our board registers. On a WRITE cycle, the latch is 
terminated by our DTACK going valid, indicating that we will be ready 
for the next access (almost) immediately after acknowledging the WRITE. 

/DTACK is the data acknowledge signal the board sends the VME Bus 
indicating that the data is ready on READ cycles or that the data has 
been stored on WRITE cycles. The basic timing for /DTACK is through 
the CYCLE and DxCYCLE signals. Together, these guarantee a minimum of 
one (1) SYSCLK cycle (62.5 ns) between addressing board registers 
(DATAIO or IMIO valid) and lowering /DTACK, or four (4) SYSCLK cycles 
(250 ns) between writing the threshold D/A and lowering /DTACK. This 
stupid D/A (AD7226) requires a write pulse of a minimum of 200 ns. DDTACK 
is the data acknowledge from accessing the the data registers, 
while IMDTACK is the data acknowledge generated by the Interrupt 
Module chip. The CYCLE, DCYCLE , etc. registers are cleared by ALATCH 
going invalid. 

Bug fixes: added ALATCH to ENDC 

modified ENDC to deassert ALATCH immediate after DTACK on 
WRITES 


CHIP 


PALPG_DTACK 


PAL20RA10 

PIN 

1 

PL 

LOW 


I 

PIN 

2 

SYSCLK 

HIGH 


I 

PIN 

3 

BSSTROBE 

LOW 


I 

PIN 

4 

VMRESET 

LOW 


I 

PIN 

5 

IMDTACK 

LOW 


I 

PIN 

6 

THWRITE 

LOW 


I 

PIN 

7 

DATAIO 

LOW 


I 

PIN 

8 

IMIO 

LOW 


I 

PIN 

9 

DS 

HIGH 


I 

PIN 

10 

AS 

LOW 


I 

PIN 

11 

W 

LOW 


I 

PIN 

12 

GND 




PIN 

13 

OE 

LOW 


?I 
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PIN 

14 

VRESET 

REG 

LOW 

0 

PIN 

15 

ALATCH 

REG 

LOW 

0 

PIN 

16 

DTACK 

LOW 


0 

PIN 

17 

DDTACK 

LOW 


0 

PIN 

18 

ENDC 

LOW 


0 

PIN 

19 

CYCLE 

REG 

LOW 

0 

PIN 

20 

DCYCLE 

REG 

LOW 

0 

PIN 

21 

D2CYCLE 

REG 

LOW 

0 

PIN 

22 

D3 CYCLE 

REG 

LOW 

0 

PIN 

23 

D4CYCLE 

REG 

LOW 

0 

PIN 

24 

VCC 





EQUATIONS 

? VRESET IS A SYNCHRONIZED VERSION OF VME RESET (VMRESET) 

VRESET = VMRESET 
VRESET . CLKF = SYSCLK 

; ADDRESS LATCH IS TRIGGERED BY BSSTROBE AND CLEARED BY ENDC 

ALATCH = VCC 

ALATCH . CLKF = BSSTROBE 

ALATCH. RSTF = ENDC 

; DATA ACKNOWLEDGE FROM THE DATA SECTION. IF NOT TO THRESHOLD WRITES, THEN 
; ONLY ONE SYSCLK DELAY. IF THRESHOLD WRITE, THEN 4 SYSCLK DELAYS. 

DDTACK = DCYCLE * DATA 10 * /THWRITE 
+ D4CYCLE * THWRITE 

; DATA ACKNOWLEDGE TO THE VME BUS IS COMPRISED OF DTACKS FROM BOTH THE DATA 
; SECTION AND THE INTERRUPT MODULE CHIP 

DTACK = DCYCLE * DATA 10 * /THWRITE 
+ D4CYCLE * THWRITE 
+ IMDTACK 

; END CONDITION FOR A BOARD ACCESS CYCLE IS DS AND AS INVALID ON READ CYCLES, 
; DTACK VALID ON WRITE CYCLES, OR VME RESET 

ENDC = /DS * /AS * ALATCH * /W 
+ DTACK * W 
+ VRESET 

; CYCLE TIMING IS INITIATED WHEN EITHER THE DATA SECTION OR INTERRUPT MODULE 
; IS BEING ACCESSED. BY SYNCHRONIZING TO SYSCLK, DCYCLE IS GUARANTEED TO BE 
; ASSERTED A MINIMUM OF ONE SYSCLK CYCLE AND A MAXIMUM OF TWO SYSCLK CYCLES 
? AFTER THE BOARD REGISTERS HAVE BEEN ADDRESSED. CYCLE AND DCYCLE ARE 
? CLEARED WHEN ALATCH IS INVALID. THE 4 TIMES DELAYED CYCLE SIGNAL, D4CYCLE, 
; IS USED BY THE DAC ON WRITES, WHICH NEEDS 200NS. D4CYCLE IS GUARANTEEING 
; A MINIMUM OF 250NS. 

CYCLE = DS * DATA 10 + DS * IMIO ; NOTE THAT DS MUST ALSO BE VALID 

CYCLE. CLKF = SYSCLK 
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CYCLE. RSTF = /ALATCH 


DCYCLE = CYCLE 
DCYCLE . CLKF = SYSCLK 
DCYCLE. RSTF = /ALATCH 

D2 CYCLE = DCYCLE 
D2CYCLE . CLKF = SYSCLK 
D2CYCLE . RSTF = /ALATCH 

D3CYCLE = D2CYCLE 
D3 CYCLE. CLKF = SYSCLK 
D3CYCLE . RSTF = /ALATCH 

D4CYCLE = D3CYCLE 
D4CYCLE . CLKF = SYSCLK 
D4CYCLE . RSTF = /ALATCH 

SIMULATION 

TRACEJDN SYSCLK DS DATA 10 IMIO THWRITE IMDTACK 

VMRESET VRESET BSSTROBE ALATCH ENDC CYCLE DCYCLE D4CYCLE DDTACK DTACK 
SETF /PL OE /SYSCLK /BSSTROBE /DS /DATA 10 /IMIO /THWRITE /IMDTACK /VMRESET 
PRLDF /ALATCH /CYCLE /DCYCLE /D2CYCLE /D3CYCLE /D4CYCLE /VRESET 
; FIRST TRY DATA SECTION ACCESS 


SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 

SETF BSSTROBE 
CHECK ALATCH 

SETF DS 
SETF DATA 10 

FOR I := 1 TO 4 DO 
BEGIN 

SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 
END 

SETF /DS 
SETF /DATA 10 
SETF /BSSTROBE 
SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 


RIGHT ACCESS 

CHECK FOR ALATCH VALID 

DATA STROBE 
DATA ACCESS 

WAIT 4 CLOCKS 


NOW TRY THRESHOLD WRITES 
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SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 

SETF BSSTROBE 
CHECK ALATCH 

SETF DATAIO THWRITE 
SETF DS 

FOR I := 1 TO 6 DO 
BEGIN 

SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 
END 

SETF /DS 

SETF /DATAIO /THWRITE 
SETF /BSSTROBE 
SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 

; NOW TRY IM ACCESS 
SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 

SETF BSSTROBE 
CHECK ALATCH 

SETF DS 
SETF IMIO 

FOR I := 1 TO 4 DO 
BEGIN 

SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 
END 

SETF IMDTACK 
SETF SYSCLK 
SETF /SYSCLK 
; CLOCKF SYSCLK 

SETF /BSSTROBE 
SETF /DS 
SETF /IMIO 
SETF SYSCLK 
SETF /SYSCLK 
? CLOCKF SYSCLK 
SETF /IMDTACK 


; RIGHT ACCESS 
; CHECK FOR ALATCH VALID 

; DATA ACCESS 
; DATA STROBE 

; WAIT 4 CLOCKS 


; MAKE CLOCK HIGH 

; RIGHT ACCESS 
; CHECK FOR ALATCH VALID 

; DATA STROBE 
; IM ACCESS 

; WAIT 4 CLOCKS 


; IM'S DTACK 
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; CHECK VRESET 

SETF VMRESET ; RAISE RESET LINE 

SETF SYSCLK 

SETF /SYSCLK 

; CLOCKF SYSCLK 

SETF SYSCLK 

SETF /SYSCLK 

; CLOCKF SYSCLK 

TRACE_OFF 


B.4.2 Buffer Enables and Device Select 


TITLE 

PATTERN 

REVISION 

AUTHOR 

COMPANY 

DATE 


PG BUFFER ENABLES AND DEVICE SELECT 
1 

2.0 + 

VINCENT CHEN 

ARL , STANFORD UNIVERSITY 

2-08-90 


This Point Grabber PAL generates enables for the data buffers (/BHEN, 

/BLEN) , device selects (/DATAIO, /IMIO, /THWRITE) , and the board select 
strobe ( /BSSTROBE ) . 

/DATAIO indicates access to the data section of the Point Grabber. 

/IMIO indicates access to the Interrupt Module. 

/BHEN is the enable for the high byte of data, and it is asserted 
only when access is to the data section on a read cycle. 

/BLEN is the enable for the low byte of data, and it is asserted 
when access is to either the data or IM sections and also during an 
interrupt cycle. When the processor is responding to an interrupt from 
the Point Grabber, the Point Grabber will receive the daisy-chained 
/VMIACKIN signal, but it will not assert /IACKOUT, thus enabling the 
data buffer. The Interrupt module will then send out the interrupt 
vector onto the VME Bus. 

BSSTROBE is the board select strobe. /CMP is the output of the 8-bit 
comparator comparing A08-A15, and VMAMx are the address modifier bits. The 
board is presently configured to fit in short 10 space, occupying a 128 byte 
block. The location of the block is determined by the input to the 
comparator generating /CMP. 


CHIP 


PALPG_EN 

PAL20L8 

PIN 

1 

CMP 

LOW 

I 

PIN 

2 

AB07 

HIGH 

I 

PIN 

3 

VMAM0 

HIGH 

I 

PIN 

4 

VMAM1 

HIGH 

I 

PIN 

5 

VMAM3 

HIGH 

I 

PIN 

6 

VMAM4 

HIGH 

I 

PIN 

7 

VMAMS 

HIGH 

I 

PIN 

8 

AO 3 

HIGH 

I 

PIN 

9 

AO 4 

HIGH 

I 

PIN 

10 

AOS 

HIGH 

I 
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PIN 

11 

AO 6 

HIGH 

I 

PIN 

12 

GND 



PIN 

13 

ALATCH 

LOW 

I 

PIN 

14 

VMIACKIN 

LOW 

I 

PIN 

15 

BSSTROBE 

LOW 

0 

PIN 

16 

IACKOUT 

LOW 

I 

PIN 

17 

D4 CYCLE 

LOW 

I 

PIN 

18 

BHEN 

LOW 

0 

PIN 

19 

BLEN 

LOW 

0 

PIN 

20 

DATAIO 

LOW 

0 

PIN 

21 

IMIO 

LOW 

0 

PIN 

22 

THWRITE 

LOW 

0 

PIN 

23 

W 

LOW 

I 

PIN 

24 

VCC 




EQUATIONS 

; BOARD SELECT HAS HIGH 128 BYTES OF A 256 BYTE BLOCK IN SHORT 10 SPACE 

BSSTROBE = CMP * AB07 * VMAMO * /VMAM1 * VMAM3 * /VMAM4 * VMAM5 

* AO 6 * /A05 * A04 

+ CMP * AB07 * VMAMO * /VMAM1 * VMAM3 * /VMAM4 * VMAM5 

* AO 6 * AO 5 * /A04 

BHEN = /W * ALATCH * A06 * /A05 * A04 

BLEN = / IACKOUT * VMIACKIN 

+ ALATCH * A06 * /AO 5 * A04 
+ ALATCH * AO 6 * A05 * /A04 

DATAIO = ALATCH * AO 6 * /AO 5 * AO 4 

IMIO = ALATCH * A06 * A05 * /A04 

THWRITE = W * ALATCH * A06 * /A05 * A04 * /A03 * /D4 CYCLE 
SIMULATION 

TRACE_ON CMP AB07 VMAMO VMAM1 VMAM3 VMAM4 VMAM5 BSSTROBE 
AO 6 AO 5 AO 4 A03 ALATCH W VMIACKIN IACKOUT 
BLEN BHEN DATAIO IMIO D4CYCLE THWRITE 

SETF /CMP AB07 VMAMO VMAM1 VMAM3 VMAM4 VMAM5 
/ALATCH AO 6 A05 A04 A03 W /VMIACKIN /IACKOUT /D4 CYCLE 

; TEST BSSTROBE 

SETF CMP 
SETF /VMAM1 
SETF /VMAM4 
CHECK /BSSTROBE 
SETF /AO 5 
CHECK BSSTROBE 
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SETF AO 5 /AO 4 
CHECK BSSTROBE 
SETF AO 4 

SETF /CMP VMAMl VMAM4 

; TEST BLEN 

SETF VMIACKIN 
CHECK BLEN 
SETF IACKOUT 
CHECK /BLEN 

SETF /VMIACKIN /IACKOUT 

SETF AO 6 /A05 A04 ; ALSO TEST DATA 10 

SETF ALATCH 
CHECK BLEN DATA 10 
SETF /ALATCH AO 5 

SETF AO 6 A05 /A04 ; ALSO TEST IMIO 

SETF ALATCH 
CHECK BLEN IMIO 
SETF /ALATCH AO 4 

; TEST BHEN 

SETF /W 

SETF AO 6 /AO 5 AO 4 ; ALSO TEST DATA 10 

SETF ALATCH 

CHECK BHEN 

SETF /ALATCH W AO 5 

; TEST THWRITE 

SETF /W 

SETF AO 6 /A05 A04 /A03 

SETF W 

SETF ALATCH 

CHECK THWRITE 

SETF /CMP ABO 7 VMAMl VMAM4 

SETF D4CYCLE 

CHECK /THWRITE 

SETF /ALATCH 

SETF A05 AO 3 /D4CYCLE 

SETF /W 

TRACE_OFF 


B.4.3 Pixel Detect 


TITLE 

PATTERN 

REVISION 

AUTHOR 


PG PIXEL DETECTOR 
1 

3.0 + 

Vincent Chen 
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COMPANY Stanford University, ARL 

DATE 07-21-90 

; This Point Grabber PAL generates the PIXel_ON signal to trigger FIFO 
; writes. It takes as input the ADC output and the Enable signal 
; for a camera and generates the PIX_0N signal 


CHIP 


PALPG_PIX 

PAL16L8 

PIN 

1 

EN 

HIGH 

PIN 

2 

ADO 

HIGH 

PIN 

3 

ADI 

HIGH 

PIN 

4 

AD 2 

HIGH 

PIN 

5 

AD3 

HIGH 

PIN 

6 

AD4 

HIGH 

PIN 

7 

ADS 

HIGH 

PIN 

8 

AD6 

HIGH 

PIN 

9 

AD7 

HIGH 

PIN 

10 

GND 


PIN 

11 

NC 


PIN 

12 

PIX_ON 

HIGH 

PIN 

13 

NC 


PIN 

14 

NC 


PIN 

15 

NC 


PIN 

16 

NC 


PIN 

17 

NC 


PIN 

18 

NC 


PIN 

19 

NC 


PIN 

20 

VCC 



EQUATIONS 

/PIX_ON = /ADO * /ADI * /AD2 * /AD3 * /AD4 * /ADS * /AD6 * /AD7 
+ /EN ;Need the enable signal 

SIMULATION 

TRACE_ON EN ADO ADI AD2 AD3 AD4 AD5 AD6 AD7 PIX_ON 

SETF /EN /ADO /ADI /AD2 /AD 3 /AD4 /AD5 /AD6 /AD7 ;init 

;test PIX_ON signal 
SETF ADO 
CHECK /PIX_ON 
SETF /ADO 
SETF EN 
SETF ADO 
CHECK PIX_ON 
SETF /ADO 
CHECK /PIX_ON 
SETF AD7 
CHECK PIX_ON 
SETF /AD7 
SETF /EN 
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TRACE_OFF 


B.4.4 Data Strobe and Resets 

TITLE 

PG STROBES AND RESETS 

PATTERN 

1 

REVISION 

2.0 + 

AUTHOR 

VINCENT CHEN 

COMPANY 

ARL, STANFORD UNIVERSITY 

DATE 

2-08-90 


This Point Grabber PAL generates the strobe signals for the camera enables 
and reset signals, as well as some auxiliary signals required to 
interface with the VME Bus. 

IACK is the interrupt acknowledge from the bus master . It is also 
asserted, along with CS to reset the interrupt module. 

ENSTB is the strobe for the camera /interrupt enable latch. 

/FFRESET is the reset for the FIFOs. 

/RESET is the reset for the threshold latches. 

DS is the data strobe for the Point Grabber, comprising of VME's 
DSl and DSO . 

/CS is the chip select for the Interrupt Module. It is asserted 
when DCYCLE becomes valid, allowing time for data to stabilize. On write 
cycles, the leading edge of /CS makes the IM latch the data, and on read 
cycles, /CS signals a data acknowledge to the IM. The Interrupt Module can 
be reset by asserting the /IACK and /CS lines, thus VME resets (VRESET) and 
software resets (WRESET) need to assert /CS. 


CHIP 


PALPG_STROBE PAL16L8 

PIN 

1 

AS 

LOW 

I 

PIN 

2 

VMIACK 

LOW 

I 

PIN 

3 

DDTACK 

LOW 

I 

PIN 

4 

WEN 

LOW 

I 

PIN 

5 

FRESET 

LOW 

I 

PIN 

6 

WRESET 

LOW 

I 

PIN 

7 

VRESET 

LOW 

I 

PIN 

8 

VMDS0 

LOW 

I 

PIN 

9 

VMDS1 

LOW 

I 

PIN 

10 

GND 



PIN 

11 

DCYCLE 

LOW i 

? I 

PIN 

12 

NC 



PIN 

13 

IMIO 

LOW i 

; I 

PIN 

14 

IACK 

LOW 

;0 

PIN 

15 

ENSTB 

HIGH 

; O 

PIN 

16 

FFRESET 

LOW 

; 0 

PIN 

17 

RESET 

LOW 

; O 

PIN 

18 

DS 

HIGH 

; 0 

PIN 

19 

CS 

LOW 

; 0 

PIN 

20 

vcc 
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EQUATIONS 

IACK = VMIACK * AS + VRESET + WRESET 

/ENSTB = /WEN + /DDT AC K 

FFRESET = VRESET + WRESET + FRESET 

RESET = VRESET + WRESET 

/DS = /VMDSO * /VMDS1 

CS = DCYCLE * IMIO + VRESET + WRESET 

SIMULATION 

TRACE_ON DDT AC K WEN ENSTB AS VMIACK WRESET VRESET FRESET FFRESET RESET IACK 
VMDSO VMDS1 DS DCYCLE IMIO CS 


SETF /WEN /DDTACK /AS /VMIACK /WRESET /VRESET /FRESET /VMDSO 
/VMDSl /DCYCLE /IMIO ; INITIALIZE 

SETF WEN 
CHECK /ENSTB 

SETF DDTACK ; WRITE ENABLE REGISTER 

CHECK ENSTB 

SETF /DDTACK /WEN 

CHECK /ENSTB 


; TEST IACK 

SETF VMIACK 
SETF AS 
CHECK IACK 
SETF /VMIACK 
CHECK /IACK 
SETF /AS 

; NOW TEST RESETS 
CHECK /FFRESET /RESET /CS 

SETF VRESET ; VME RESET 

CHECK FFRESET RESET CS 
SETF /VRESET 

SETF WRESET ; BOARD RESET 

CHECK FFRESET RESET CS 
SETF /WRESET 


SETF FRESET 

CHECK FFRESET /RESET /CS 


FIFO RESET 
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SETF /FRESET 

CHECK /DS 
SETF VMDSO 
CHECK DS 
SETF /VMDSO 
SETF VMDSl 
CHECK DS 
SETF /VMDSl 
CHECK /DS 

SETF IMIO 
SETF DCYCLE 
CHECK CS 
SETF /DCYCLE 
CHECK /CS 

SETF /IMIO 
TRACE_OFF 


; TEST DS 


; NOW TEST CS 


B.4.5 Horizontal and Vertical Syncs 


TITLE 

PATTERN 

REVISION 

AUTHOR 

COMPANY 

DATE 


PG SYNC GENERATOR , INTERRUPT AND FIFO WRITE 
1 

3.0 + 

VINCENT CHEN 

ARL , STANFORD UNIVERSITY 

07-21-90 


; Requires 20ns PAL, or SEEQ PQ20RA10Z-35 

; This Point Grabber PAL provides the sync signals to the cameras as well 
; as the interrupt signals at the end of each frame and the write pulse 
; to the FIFOs at each edge and at the end of each frame. 


CHIP 


PALPG_SYNC 


PAL20RA10 

PIN 

1 

PL 

LOW 


I 

PIN 

2 

XCLK 

HIGH 


I 

PIN 

3 

YCO 

HIGH 


I 

PIN 

4 

YC02 

HIGH 


I 

PIN 

5 

XCO 

HIGH 


I 

PIN 

6 

XC06 

HIGH 


I 

PIN 

7 

XC02 

HIGH 


I 

PIN 

8 

IMDTACK 

LOW 


I 

PIN 

9 

INTEN 

HIGH 


I 

PIN 

10 

PIX_ON0 

HIGH 


I 

PIN 

11 

PIX_ONl 

HIGH 


I 

PIN 

12 

GND 




PIN 

13 

OE 

LOW 



PIN 

14 

HORIZ 

REG 

HIGH 


PIN 

15 

HSTB 

REG 

LOW 
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PIN 

16 

HRST 

REG 

LOW 

PIN 

17 

VERT 

REG 

HIGH 

PIN 

18 

VSTB 

REG 

LOW 

PIN 

19 

VRST 

REG 

LOW 

PIN 

20 

FRAME 

REG 

LOW 

PIN 

21 

FRST 

HIGH 


PIN 

22 

FWRITE 

REG 

LOW 

PIN 

23 

IRQ 

REG 

LOW 

PIN 

24 

VCC 




EQUATIONS 

HSTB = XCO 
HSTB. CLKF = XCLK 

HORIZ = VCC 
HORIZ . CLKF = HSTB 
HORIZ. SETF = HRST 

HRST = XCO 6 
HRST. CLKF * XCO 2 
HRST.RSTF = /HORIZ * HRST 


VSTB = YCO 
VSTB.CLKF = XCLK 

VERT = VCC 
VERT. CLKF = VSTB 
VERT. SETF = VRST 

VRST = VCC 
VRST. CLKF = YC02 
VRST.RSTF = /VERT * VRST 


IRQ = INTEN 
IRQ. CLKF = VERT 
IRQ . RSTF = IMDTACK 

FRAME = VCC 
FRAME. CLKF = VERT 
FRAME. RSTF = FRST 

FRST = /INTEN + FWRITE * XCLK 


FWRITE = FRAME + PIX_ONO + PIX_ONl 

FWRITE. CLKF = /XCLK 
FWRITE. RSTF = FRST 


0 

0 

0 

0 

0 

0 

0 

0 


; STROBE TO LATCH HORIZ SYNC 
; CLOCKED BY XCLK 

; HORIZ SYNC 
; LATCHED BY HSTB 
; RESET BY HRST 

; RESET FOR HORIZ SYNC 
; CLOCKED BY XC02 FOLLOWING XCO 6 
; CLEARED BY HRST AND AFTER 

; HORIZ SYNC IS CLEARED 

; STROBE TO LATCH VERTICAL SYNC 
; CLOCKED BY XCLK 

; VERTICAL SYNC 
; LATCHED BY VSTB 
; RESET BY VRST 

; RESET FOR VERTICAL SYNC 
; CLOCKED BY YC02 {2 LINES) 

; CLEARED BY VRST AND AFTER 

; VERTICAL SYNC IS CLEARED 

; IRQ GENERATED ONLY WHEN ENABLED 
; CLOCKED BY VERT 
; CLEARED BY IMDTACK 

; FRAME MARKER WRITE STROBE 
; CLOCKED BY VERT 
; CLEARED BY FRST 

; FRAME MARKER STROBE RESET WHEN 
; INTERRUPTS NOT ENABLED AND AFTER 
; WRITE PULSE 

; FIFO WRITE PULSE WHEN FRAME OR 
; ANY PIXELS ON 
; CLOCKED ON /XCLK (DELAY) 

; CLEARED IF INTERRUPTS NOT 
; ENABLED 
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SIMULATION 

TRACE_ON XCLK XCO XC06 XC02 HSTB HORIZ HRST YCO YC02 VSTB VERT VRST 
INTEN IMDTACK IRQ PIX_ONO PIX_ONl FRAME FWRITE FRST 

SETF /PL OE /XCLK /YCO /YC02 /XCO /XC06 /XC02 INTEN /IMDTACK /PIX_ONO /PIX_ONl 

PRLDF VSTB /VERT VRST HSTB /HORIZ HRST IRQ FRAME FWRITE 

; TEST HORIZ SYNC 

SETF XCO 
SETF XCLK 
SETF /XCLK 
CHECK HSTB 
SETF /XCO 
CHECK HORIZ 
SETF XCO 6 
SETF XCLK 
SETF /XCLK 
SETF XC02 
SETF XCLK 
SETF /XCLK 
CHECK /HORIZ 
SETF /XCO 6 /XCO 2 
SETF XCLK 
SETF /XCLK 

; TEST VERTICAL SYNC AND IRQ 

SETF INTEN 
SETF XCLK 
SETF /XCLK 

SETF YCO 
SETF XCLK 
CHECK VSTB 
SETF /YCO 
CHECK VERT 
CHECK IRQ FRAME 
SETF /XCLK 
CHECK FWRITE 
SETF XCLK 
SETF /XCLK 
CHECK /FRAME 
SETF YCO 2 
SETF XCLK 
SETF /XCLK 
CHECK /VERT 
SETF XCLK 
SETF /XCLK 
SETF /YCO 2 
SETF XCLK 
SETF /XCLK 
SETF IMDTACK 
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CHECK /IRQ 
SETF XCLK 
SETF /XCLK 
SETF / IMDTACK 

; TEST INTEN ON VERTICAL SYNC 

SETF /INTEN 

SETF YCO 
SETF XCLK 
CHECK VSTB 
CHECK VERT 
CHECK /IRQ /FRAME 
SETF /YCO 
SETF XCLK 
SETF /XCLK 

; TEST PIX_ONO WRITES 

SETF INTEN 

SETF XCLK 
SETF PIX_ONO 
CHECK /FWRITE 
SETF /XCLK 
CHECK FWRITE 
SETF /PIX_ONO 
SETF XCLK 
CHECK /FWRITE 
SETF /XCLK 

SETF /INTEN 

SETF XCLK 
SETF PIX_ONO 
SETF /XCLK 
CHECK /FWRITE 
SETF /PIX_ONO 

; TEST PIX_ONl WRITES 

SETF INTEN 

SETF XCLK 
SETF PIX_ONl 
CHECK /FWRITE 
SETF /XCLK 
CHECK FWRITE 
SETF /PIX_ONl 
SETF XCLK 
CHECK /FWRITE 
SETF /XCLK 


SETF /INTEN 
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SETF XCLK 
SETF PIX_0N1 
SETF /XCLK 
CHECK /FWRITE 
SETF /PIX_0N1 


TRACE_OFF 



Appendix C 


Calibration 


All sensor and actuator signals are sampled by the Analog-to-Digital (A/D) and Digital-to-Analog (D/A) 
circuitry before being passed to the controller. The analog signals from the sensors and to the actuators 
are adjusted such that lull-scale readings on the A/D’s and D/A’s correspond with the maximum outputs 
and inputs of the sensors and actuators, respectively. The controller, therefore, must provide at least a 
gain and an offset for each sensor and actuator signal to transform them into the proper units. 

Nonlinearities in the sensors or actuators, however, can make a single gain and offset for each 
device inadequate. Calibration with a polynomial correction function can often reduce the effects of 
the nonlinearities. Three areas where polynomial fits made a significant improvement in the Multi- 
Manipulator Free-Flying Space Robot experiment are the camera-lens-distortion correction, the RVDT 
joint-angle nonlinearity compensation, and the motor-torque-curve compensation. Accordingly, this 
appendix is divided into three sections to show the improvement of a polynomial fit in each case. 

C.l Camera-Lens-Distortion Correction 

The camera lens used in the global vision system is a 6mm wide-angle lens. There are, therefore, some 
barrel-distortion effects around the edges. Since the lens is circularly symmetric, this distortion is a 
function of the radius. To correct for this distortion, the calibration procedure performs a polynomial 
fit as a function of both the x and y coordinates. That is: 

x c = f Vm) (Cl) 

He — fy{ x my Vm) 
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where x m and y m represent the measured x and y coordinates of a point, x c and y c are the corrected 
values, and f x and f y are the polynomial correction functions. This correction is equivalent to 
performing the correction as a function of the radius, since the radius is itself a function of x and y: 

Tm = \/4> +S4 ( C2 ) 

Moreover, performing this calibration also reduces errors caused by camera tilt and rotation. 

A large plate containing a grid of 8 X 16 evenly-space LED’s is used for calibration 1 . The LED s are 
spaced at 6 inches and a complete grid of 12 X 16 LEDs is needed to cover the field of view of a camera. 
The calibration plate can be shifted to provide the full set of data, and the overlapping rows are used 
to accurately align the two calibration plate positions. The calibration routines find the coefficients of 
polynomials that minimize the errors between measured LED locations and the actual known locations 
of the LED s on the calibration plate. 

The functions of a third-order polynomial fit, for example, have the form: 

X> c ~~ ^xo x m ~b ( ~'X\ x m ~b c xi x m "I” Vm "b Vra "b ^x^Vvn 

"f c x 6 X my m "b c X7 X mVm + Cx8 X m2/m “b C x 9 (C.3) 

Vc — c yo x m "b c yi x m “b c yi x m “b c y$y m + Cy^m “b Cy^m 

“b c y^ x mV m *b c y7 X my r n “b c y% x mym + Cy 9 (C.4) 


Given the polynomial order n, the number of coefficients is M = (n + l)(n + 2)/2, thus the third-order 
fit has ten coefficients for each coordinate. 

The coefficients are determined by solving a least-squares problem. Each row i of the regressor 
matrix A is formed using a measured data pair (x mi , y mt ) as: 


X m x x m t 2/m t 1 


(C.5) 


Denoting the actual location of the ith LED as (x 00 y fli ), the least-squares problem for a collection of 
N measured data sets is then formed as: 


Xoi Vaj 




Imj 

Vm, 

Vmj 

Vmj I mj Vmi 

x mi 

imj Vm i 1 

- Xa N Va N - 


x 3 

. m A r 

X ™N 

Xrn N 

Vm N 

Vm N 

Vm^r x mpf Vrn N 

Xrn !\’ y rn^ 

Xf ny Vm 1 


C XQ Cy Q 


C *M C VM -I 


1 Thc calibration plate is built by Kurt Zimmerman. 




196 


Appendix C. Calibration 


Solving the least-squares problem for C gives both sets of coefficients to use for lens-distortion 
correction. This can be performed easily with a single command in MATLAB: C = A\X. 

Figure C.l shows the effects of first-order, second-order, and third-order polynomial fits. The left 
column shows the actual LED locations as os and the corrected locations as *+s for each order fit. 
The right plots show, qualitatively, the errors in each fit in a three-dimensional view 2 . The error plots 
for the first- and second-order fits show clearly the circularly-symmetric effect of the lens distortion. 
The plots for the third-order fit show the dramatic improvement. Using even higher order fits does 
not significantly improve the fit, and has the disadvantage of having too many coefficients, slowing the 
computation. 

The MATLAB code for performing the polynomial fits follow. 


2 The upper left data point in each error plot is a dummy value to preserve the scaling throughout the three plots. 





Y Position (m) Y Position (m) Y Position (m) 
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Figured: Lens-Distortion Calibration 

These are the lens-distortion correction results for three different polynomial fits . The left plots 
show the actual LED locations as the o s and the corrected measured locations as ‘+ s. Jr can 
be seen that there is significant improvement with the third-order polynomial . The right plots 
show the errors in a three-dimensional view. The upper-right corner of each error plot contains 
a dummy data point, used as a reference to preserve the scaling throughout all three plots. 
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C. 1 . 1 Polynomial Fit to Two Variables 


function p = polyf it2 (x,y, n) 

% POLYFIT2 POLYFIT2 (x, y, n) finds the coefficients of 
% two-variable polynomials formed from the data in 

% matrix x of degree n that fits the data in matrix y in 

% a least-squares sense. Each column of x represents a 

% variable, and a polynomial is fit to each column of y. 

% 


% Vincent Chen 1-4-90 

% Revised 7-31-90 for only 2 variables w/ cross terms 
% Copyright (c) 1990 


% the regression problem is formulated in matrix format as: 
% 


% y 
% 

% 

% y 
% 

% 

% 

% 

% 

% 

% 

% 

% 

% 


A*P or 

3 2 3 2 2 2 

[xl xl xl x2 x2 x2 xl x2 xlx2 xlx2 1] [pl3 

pl2 

pH 

p23 

p22 

p21 


p0 ] 


% where the matrix P contains the coefficients to be found. Each column 
% of P contains the coefficients for a fit to the corresponding column 
% of y. NOTE that there is only one constant term coefficient, pO . 

% For a 7th order polynomial of a single variable, matrix A would be: 


% 

% A = [x.~7 x.~6 x.~5 x.~4 x.~3 x.~2 x ones (x) ] ; 

% 


% See also polyva!2 


xs = s i z e ( x ) ; 
ys = size (y) ; 
if ( xs ( 1 ) ~= ys ( 1 ) ) 

error ('X and Y must have the same number of rows') 

end 


if ( (xs (2 } ~= 2) | (ys ( 2 ) ~= 2) ) 

error ('X and Y must have exactly 2 columns') 

end 


% A must have same number of rows as x and each column must be 
% repeated n times 
Ancol = 0; 
for i = 0:n 

Ancol = Ancol + i + 1; 
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end 

if ( xs ( 1 ) < Ancol ) 

error ('Not enough data to perform the fit, decrease n') 

end 

A = ones(xsd), Ancol); 

% Construct Vandermonde matrix 
for i = 1 : 2 
for j = l:n 

A ( : , n* ( i-1 ) +j ) = x ( : , i ) . ~ (n+l-j ) ; 

end 

end 


Aindex = 2*n + 1; 
for i = l:n-l 

for j = 2*n- ( i-1 ) : 2 *n 

A {:, Aindex) = A ( : , i+1 ) . *A ( : , j ) ; 
Aindex = Aindex + 1; 

end 

end 

p = (A\y) ; 


C. 1 .2 Evaluation of Polynomial of Two Variables 


function y = polyval2 (c , x) 

% P0LYVAL2 Polynomial evaluation. 

% If C is a matrix whose column elements are the coefficients of a 

% polynomial, then POLYVAL2 (C , X) is the value of the 

% polynomials evaluated at X. If X is a vector, 

% the polynomial is evaluated at all points in X. 

% If X is a matrix, then each column of X represents a variable 
% in a two-variable polynomial, and each column of C contains 
% the coefficients of the variables in succession. For a two 
% variable, 3rd order polynomial, a column of C looks like: 

% 

% c = [cl3 

% cl2 

% ell 

% c23 

% c22 

% c21 

% c021 

% c012 

% cOll 

% cO] 

% 

% and if X is [xl x2] , then 
% 

% 


3 


2 
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% y = cl3 * xl + cl2 * xl + ell * xl 

% 

% 3 2 

% + c23 * x2 + c22 * x2 + c21 * x2 

% 

% 2 2 
% + c021 * xl * x2 + c012 * xl * x2 

% 

% + cOll * x2 * x2 

% 

% + cO 

% Polynomial evaluation c(x) using Horner's method 

% Vincent Chen 1-5-90 
% Revised 
% Copyright 1990 

% See also polyfit2 

[m, n] = size (x) ; 

[me, nc] = size (c) ; 

order = 0 ; 
ocount = 1 ; 
while ocount < me, 

order = order + 1 ; 

ocount = ocount + order + 1; 

end 


if ( ocount ~= me ) 

error (' column size of C does not match any order of polynomials') 

end 

if (m+n) == 2 

% Make it scream for scalar X. Polynomial evaluation can be 
% implemented as a recursive digital filter, 
y = zeros { 1 , nc) ; 
for i = l:nc 

yy = filter(l, [1 -x],c(:,i)); 
y (1, i) = yy (nc) ; 

end 

return 

end 

% Do general case where X is an array 
y = zeros (m, nc) ; 
for j=l:n 

yy= zeros (m, nc) ; 
for i=l: order 

yy = (x(:,j) * ones(l,nc)) . * yy. . . 

+ ones {m, 1) * c ( ( ( j -1 ) *order) +i , : ) ; 

end 

y = y + (x(:,j) * ones(l,nc)) .* yy; 
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end 

cindex=2 *order+l ; 

for i = order-1: -1:1 

for j = order-i:-l:l 

y = y + { (x ( : , 1) * ones(l, nc)) / i).* ((x(:,2) * ones(l, nc)). A j 
(ones{m, 1) * c (c index, : ) ) ; 
cindex = cindex + 1? 

end 

end 

y = y + ones(m,l) * c( me,:); 

C.2 Joint-Angle Calibration 

The RVDT’s used to measure joint angles also contain some nonlinearity. A simple third-order 
polynomial as a function of the a single variable — the measured joint-angle — is used for each joint 
RVDT. 

A calibration jig, containing holes in know locations, is bolted to the robot base. The end effector 
of each manipulator is place in each of the holes for RVDT readings. These are matched with the joint 
angles derived from exact inverse-kinematics equations. 

Figure C.2 shows the errors of using a linear fit vs. a third-order fit for each joint. In all cases, there 
is noticeable improvement. 

C.3 Torque-Curve Calibration 

The Aeroflex brushless DC motors used to actuate the space robot manipulators deliver smooth torques 
with very low friction. The motors are, however, limited-angle torquers, and the delivered torques for 
a given input current drop as the motors rotates away from zero angle. This effect is symmetric around 
the zero angle of each motor, and can be minimized by introducing a scaling function that is dependent 
on the motor angle. A scaling function essentially boosts the motor current to achieve the requested 
output torque for all motor angles. 

The torque curve, i.c., scaling function, can be modelled as a fourth-order polynomial, restricted to 
having a single inflection point at the central motor angle. Since the controller has direct access to the 
joint-angles rather than motor angles, the calibration curves are derived as functions of the joint-angles. 



error (deg) error (deg) 
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Figure C.2: Joint-Angle Calibration Errors 

These plots show the calibration errors for a linear fit vs. a third-order fit for each joint. They 
clearly show the improvement with the third-order fits. 


They have the form: 

SCulCi s= C 4 j ((ft — 9ofiset i) P 2i (?t — 9ofiscti) “h 1 (C.6) 

where g 0 flfscti I s the angle of joint i that corresponds to the zero angle of motor i, and and C2i are 

the coefficients to be derived from least-squares fits 3 . The requested torques from the control laws are 

multiplied by these scale factors before sending them to the motor drivers. 

Figures C.3 and C.4 show typical calibration curves. The data was taken by measuring the requested 

torques necessary to cancel the effects of a known applied torques at different joint angles. The os 

3 Strictly speaking, cu should be zero to force a single inflection point in the torque curve, but its presence can improve 
the fit. 
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indicate the values of the applied torques, the ‘+ s indicate the torques that the control law requests, and 
the xs indicate the requested torques after fourth-order fit. For a perfect fit, the Vs and the xs should 
overlap. The plots show that the fit is very reasonable. 



Figure C.3: Right Shoulder Motor Calibration Curves 


The lines in Figures C.3 and C.4 plot the torque curve as a continuous function of the joint angle for 
the three values of applied torques. Ideally, these torque curves should intersect the V marks, and the 
figures show that the torque curves the torque curves do fit the measured values, indicating again that 
the fourth-order calibration polynomial is a good model for the behavior of the manipulator motors. 

C.3. 1 Torque Calibration File 

% Name: 

% torquecal.m - motor torque calibration 

% 

% Description: 

% The first section lists the measured data and the rest performs 
% the calibration. 

% 

!rm *.met 

if arm == 0 % Right Shoulder 
% Data 

t0=-[-.06 -.015 .01 .03 .05] ; 
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Figure C.4: Left Shoulder Motor Calibration Curves 


tl = - [ . 22 .19 .21 .23 .27]; 
t2 = - [ . 46 .375 .385 .42 .48]; 
t3 = - [ . 70 .555 .56 .61 .69]; 

% The angles at which the data were taken 
ang0= [-1.75 -1.25 -.78 -.25 .25]; 

% Gain for the motor 
sc = -0.8272; 

% Joint angle corresponding to motor's zero angle 
qO = -.65; 

% Plotting axes and title text 

axis { [ -2*180/pi .5*180/pi 0 .7]); 
tl= ' Right ' ; 

else % Left Shoulder 

% Data 

t0=- [ - . 05 -.04 .0 .04 .1] ? 
tl=- [ - . 28 -.25 -.23 -.18 -.21]; 
t2=- [ - . 5 -.43 -.395 -.38 -.47]; 
t3=- [- . 725 -.625 -.58 -.55 -.73); 

% The angles at which the data were taken 
ang0= [ - . 25 .25 .78 1.25 1.75]; 

% Gain for the motor 
sc = .7872; 

% Joint angle corresponding to motor's zero angle 
qO = .65; 

% Plotting axes and title text 

axis ( [- . 5*180/pi 2*180/pi 0 .7]); 
tl= ' Lef t ' ; 


end 
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% Set up data stream, subtracting the effects of the spring forces. 
t= [ tl-tO t2-t0 t3-t0] ' *sc; 
tu— [ tl t2 t3 ] ' * sc; 

% Set up the ideal data set. 
idealO= [ 0 0000]; 
ideal 1= . 165*ones (1,5) ; 
ideal2= . 314*ones (1,5) ; 
ideal 3= . 464*ones (1,5) ; 

ideal= [ ideall ideal2 ideal3]'; 

ang=[ang0 angO angO]'; 

% Perform the fit using either: 

% c4 x~4 + c2 x~2 + cO 

% or 

% c4 x"4 + cO 

% 

if fourth == 0 

c=polyf it ( (ang-ones (ang) *q0 ) . ~2 , ideal. /t, 2); 
cl= [c ( 1) 0 c (2 ) 0 c ( 3 ) ) ; 
cor=t . *polyval (cl, (ang-ones (ang) *q0) ) ; 
coru=tu. *polyval (cl, (ang-ones (ang) *q0) ) ; 

else 

c=polyf it ( (ang-ones (ang) *q0 ). "4 , ideal. /t, 1); 

cl= [c ( 1 ) 0 0 0 c ( 2 ) ] ; 

cor=t . *polyval (cl , (ang-ones (ang) *q0 ) ) ; 
coru=tu . *polyval (cl , (ang-ones (ang) *q0) ) ; 

end 


% Setting up for plots 
names= [ 'Actual ' 
'Measured' 

'Fit '3; 

1 types = [ 'o' 

' x ' ] ; 


plot (ang* 180/pi , t, '+b'); hold on; grid; 
plot (ang*180/pi , cor, 'xr'); 
plot (ang* 180/pi , ideal, 'ow'); 
err=cor-ideal ; 
maxerr=max (err ) ; 


a=[-4: .01:4] ; 
s=polyval (cl , a) ; 

plot ( [a; a; a] ' *18 0/pi, ( [ . 165; . 314; . 464] * (ones (a) . /polyval (cl, a-ones (a) *q0) ) ) ' ) ; 
hold off 

title ((tl '-Shoulder Torque Calibration']) 

xlabel ( 'Shoulder Angle (deg) '), ylabel ( 'Torque (N-m) ' ) 

c=legndbig (names ,0,0,1000,1, ltypes ) ; 

pause 


Appendix D 


State Transition Diagrams 


This appendix contains the state-transition graphs for the Multi-Manipulator Free-Flying Space Robot 
experiment. Implementing the transition graphs allows the robot to complete complicated tasks 
autonomously. Although not explicitly indicated, all control is performed utilizing the adaptive task- 
space control structure. 

The graphs depict the states as ovals, and the state transitions as arrows. A stimulus-and-transition- 
routine pair is labeled at the base of each arrow; the pair is separated by a character. If a transition 
routine returns more than one value, causing a branch in the transition, the return values are labeled 
near the arrow heads. 

The state-transition graphs are separated into sue (6) main functions: “initialization”, “payload 
capture”, “rendezvous with object”, “deliver object”, “object motion”, and “robot motion”. Whenever 
the robot is enabled, it executes the initialization sequence, checking the health of the system. The 
robot carries out a payload capture in response to user request; if the payload object is not in range, the 
robot first performs a rendezvous with the object. If the user requests the movement of the payload, the 
robot executes the delivery of the object. It must also execute the “object motion” transitions to check 
the final orientation of the payload; if the orientation is not achievable with the current manipulator 
grasp, a swap is performed. Finally, if the user requests a base movement, the robot executes the “robot 
motion” transitions. 
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Figure D.l: Initialization Transition Graph 

The robot system starts in the “Init” stare and checks the safety switc/i, gas pressure , and 
electrical voltage when sampling is started. When everything checks our, the grippers are raised 
and the system makes sure that the robot base is being tracked. The motors and joint controller 
are then enabled to bring the arms into a known “home” location. The local vision system must 
Hnd the endpoints before entering the “Ready” state. If an error occurs , the robot enters the 
“Error” state, which transitions back into the “In it” state, and the process repeats. Additionally, 
three global stimuli — Abort , Singularity and Sam plingStopped — brings the system to “I nit" 
from any other state. 
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Figure D.2: Payload-Capture Transition Graph 

The robot awaits the “Capture” or “De/iver” command in the “Ready” state, either of which 
initiates the payload-capture transition routines. If the object is not in reach , die robot 
branches to the “Rendezvous” set of transitions, Odierwise, the robot slews the arms to the 
object — tracking the grip ports using PID control until the tolerance is met — before lowering 
the grippers. The object is stopped smoothly ending in the “ Holding Object” state. If 
the command was “De/iver” die robot proceeds with the “Object Delivery ” transitions. A 
“ Release ” command forces the transition from “ Holding Object ” to “ Raising Grippers ” , which 
is part of the “ Initialization ” transition graph. 
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* Stales marked with an asterisk transition to the “Error” state in response to the 
“ObjectOutOfV'icw AND ObjectLost” stimulus. The transition routine is lostObjectQ. 


Figure D.3: Object-Rendexvous Transition Graph 

The robot must rendezvous with the object if it is out of reach. The robot plots an intercept 
trajectory, and moves toward the object. The robot continues to update the base trajectory 
until the object comes into view of the local vision system. If at any time, the robot decides 
that it cannot reach the object, the robot aborts into the Error state. When the object comes 
into view ; the robot waits for the the object to come into reach and slews the manipulators to 
the object and enters the “Payload-Capture” transition graph. An Error occurs if the object 
is lost by the vision systems. 
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If the robot is to deliver the payload and the destination is in the reach of the manipulators , 
the robot goes directly to the “ Object-Motion ” set of transitions. Otherwise, it must stow the 
object before plotting a base trajectory toward the destination. The robot continues to update 
the base trajectory until it reaches the destination. If at any time, the robot determines that the 
destination is out of range, it aborts with an “Error” condition. After reaching the destination 
of the robot base, the robot proceeds with the “Object-Motion” transitions. 
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Figure D.5: Object-Motion Transition Graph 

The first step in moving the payload object into its 6nal position is to check the grip orientation. 
I fit is achievable, the robot directly moves the payload into position and returns to the “Holding 
Object” state . Otherwise, the robot either swaps the grip directly ; or first moves the payload in to 
a favorable position before performing the swap. The swap is performed by releasing the object, 
homing the arms, and recapturing the payload by looping back to the “Starting Capture” state. 
Because the “Deliver" command is still in effect, the complete capture-and-delivery procedure 
is repeated. 
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Figure D.6: Robot-Motion Transition Graph 

This is the simplest transition of all. If the destination is reachable , the trajectory is computed , 
and the robot base is moved . Otherwise , the command is ignored. The robot ends up in the 
“Ready” state. 




Appendix E 


Setup Files 


This appendix provides a sampling of the setup files used by the Multi-Manipulator Free-Flying Space 
Robot during initialization. These files contain, for instance, the nominal physical parameters for the 
robot, the adaptive update gains, and the controller gains for each control mode. Many more data files 
are required to provide the full capabilities of the space robot. 


E. 1 Physical Parameters 

# Name : 

# properties.dat 

# 

# Description: 

# physical properties of the robot 

# 

# Written by: Vincent Chen November 1990 

#____ 


signalSet Masses 


BaseMass 

kg 

62.265 

RAUpperLinkMass 

kg 

1.9231 

RAX. o we r L i nkMa s s 

kg 

0.3382 

LAUpperLinkMass 

kg 

1.9231 

LAX owe r L i nkMa s s 

kg 

0.3382 

PayloadMass 

kg 

1.01 

LargePayloadMass 

kg 

8.87 

signalSet Inertias 

Baselnertia kg-m~2 

3.29218 

RAUpperLinklnertia 

kg-m~ 2 

0.02379 

RAXowerLinklnertia 

kg-irT 2 

0.00416 

LAUpperLinklnertia 

kg-irT2 

0.02379 

XAXowerLinklnertia 

kg-m" 2 

0.00416 
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Payloadlnertia kg-m"2 0.007 

LargePayloadlnertia kg-m~2 0.108 


signalSet 

Lengths 


L_Clx 

m 

0.184095 

L_Cly 

m 

-0.184095 

L_C2x 

m 

0.184095 

L_C2y 

m 

0.184095 

L_ll 

m 

0.3048 

L_12 

m 

0.2959 

L_21 

m 

0.3048 

L_22 

m 

0.2959 

L_C0x 

m 

0 

L_C0y 

m 

0 

L_llx 

m 

0.0594 

L_lly 

m 

-0.002 

L_12x 

m 

0.1058 

L_12y 

m 

0 

L_21x 

m 

0.0594 

L_21y 

m 

0.002 

L_22x 

m 

0.1058 

L_22y 

m 

0 

L_OBJx 

m 

0 

L_OBJy 

m 

0 

L_GRIPrx 

m 

0.111 

L_GRIPry 

m 

0 

L_GRIPlx 

m 

- 0.111 

L_GRIPly 

m 

0 


signalSet 


Spr ingCons tants 


RS_SpringK 
RE_SpringK 
LS_SpringK 
LE^SpringK 
RS^SpringOf f set 
RE_SpringOf f set 
LS_SpringOf f set 
LE^SpringOf f set 


N-m/rad 0 
N-m/rad 0 
N-m/rad 0 
N-m/rad 0 
rad -.78 

rad 1.57 

rad .78 

rad -1.57 


E.2 Adaptive Control Gains 

# Name : 

# BTAdaptSigset.dat 

# 

# Description: 

# Adaptive Controller parameter vector and gains. 

# 

# Written by: Vincent Chen 26 October 1990 

# 

signalSet parameters 
Theta_0 
Theta_l 
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- 

Theta_2 

Theta_3 

Theta_4 

Theta_5 

Theta_6 

Theta_7 

Theta_8 



Theta_9 

Theta_10 

Theta_ll 

Theta_12 

Theta_13 

Theta_14 



Theta_15 

Theta_16 

Theta_17 

Theta_18 

Theta_19 

Theta_20 

Theta__21 



Theta_22 

Theta_23 

Theta_24 

Theta_25 

Theta_26 

Theta_27 



signalSet ParametersMax 28 


ThetaMax_0 

100 


ThetaMax_l 

2 


ThetaMax_2 

2 


ThetaMax_3 

1 


ThetaMax_4 

.1 


ThetaMax_5 

1 


ThetaMax_6 

.1 


ThetaMax_7 

1 


ThetaMax_8 

.1 


ThetaMax_9 

1 


ThetaMax_10 

.1 


ThetaMax_ll 

10 


ThetaMax_12 

1 


ThetaMax_13 

.1 


ThetaMax_14 

1 


ThetaMax_15 

.1 


ThetaMax_16 

20 


ThetaMax_17 

.1 


ThetaMax_18 

.1 


ThetaMax_19 

1 


ThetaMax_20 

.05 


ThetaMax_21 

.05 


ThetaMax_22 

.05 


ThetaMax_23 

.05 


ThetaMax_24 

.05 


ThetaMax_25 

.05 


ThetaMax_26 

.05 
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ThetaMax_27 .05 

signalSet ParametersMin 28 
ThetaMinJD 0 
ThetaMin_l -2 
ThetaMin_2 -2 
ThetaMirO 0 
ThetaMin_4 - . 1 
ThetaMin_5 0 
ThetaMin_6 - . 1 
ThetaMin_7 0 
ThetaMin_8 - . 1 
ThetaMin_9 0 
ThetaMin_10 -.1 
ThetaMin_ll 0 
ThetaMin_12 0 
ThetaMin_13 0 
ThetaMin_14 0 
ThetaMin_15 0 
ThetaMin_16 0 
ThetaMin_17 -.1 
ThetaMin_18 - . 1 
ThetaMin_19 0 
ThetaMin_20 -.05 
ThetaMin_2 1 -.05 
ThetaMin_22 -.05 
ThetaMin_23 -.05 
ThetaMin_24 -.05 
ThetaMin_25 -.05 
ThetaMin_26 -.05 
ThetaMin_27 -.05 

signalSet ParameterAdaptiveGains 28 


Gamma_0 

0 

Gamma_l 

0 

Gamma_2 

0 

Gamma_3 

0 

Gamma_4 

0 

Gamma__5 

0 

Gainma_6 

0 

Gainma_7 

0 

Gamma_8 

0 

Gamma_9 

0 

Gamma_10 

0 

Gamma_.ll 

0 

Gamma_12 

0 

Gamma_13 

0 

Gainma_14 

0 

Gamma_15 

0 

Gamma_l 6 

250 

Gamma_17 

.01 

Gamma_18 

.01 

Gamma_19 

.06 

Gamma_20 

0 

Gamma_21 

0 
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Gamma_2 2 0 
Gamma_2 3 0 
Gamma_2 4 0 
Gamma_2 5 0 
Gamma_2 6 0 
Gamma_2 7 0 


signalSet SpringUpdateGains 8 


RSK 0 

RSO 0 

REK 0 

REO 0 

LSK 0 

LSO 0 

LEK 0 

LEO 0 


signalSet BTNumParmsUpdate 1 

nParmsUpdate 28 


# trailing was 
adapt iveDataSet 
nUpdate : 
trailing : 
params : 
maxParams : 
minParams : 
gains : 
errors : 
regressor : 
transpose : 


1 

BTRobotParms 28 
BTNumParmsUpdate 
1 

Parameters 
ParametersMax 
Parameter sMin 
ParameterAdaptiveGains 
JinvFilteredErrors 
Yy 
1 


Sample 


E.3 Base-Relative Object-Control Gains 

# Name : 

# objectControlBR.dat 

# 

# Description; 

# Base-relative object control signals and gains. 

# 

# Written by: Vincent Chen May 1991 

# 

signalSet DesObjectPosBR 2 

desObjectPosXBR meters 

desObjectPosYBR meters 

signalSet DesOb jectOrientBR 1 

desOb jectOrientBR radians 

signalSet DesObjectVelBR 

desOb j ectVelXBR m/sec 

desOb j ectVelYBR m/sec 


2 
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signalSet DesOb jectAngVelBR 1 

desObjectAngVelBR rad/sec 

signalSet DesObj ectAccBR 2 

desOb jectAccXBR m/sec~2 

desObjectAccYBR m/sec~2 

signalSet DesObj ectAngAccBR 1 

desObjectAngAccBR rad/sec "2 

signalSet ObjectPosGainsBR 2 

objectPosGainXBR 55 

objectPosGainYBR 55 

# Note velocity gain selected for critical damping: Kv = 2 * sqrt(Kp) 

signalSet ObjectVelGainsBR 2 

objectVelGainXBR 15 

obj ectVelGainYBR 15 


# Note: These gains for the object are unstable if you use the 2Hz estimator 

# and vision-based velocity feedback. A 5Hz estimator works fine. 

signalSet ObjectOrientGainsBR 1 

objectOrientGainBR .55 

# Note velocity gain selected for critical damping: Kv = 2 * sqrt{Kp) 

signalSet ObjectAngVelGainsBR 1 

obj ec t AngVe IGa inBR . 1 5 

signalSet ObjectPosFeedbackBR 2 

obj ectPosFeedbackXBR 
obj ectPosFeedbackYBR 

signalSet Obj ectOrientFeedbackBR 1 

obj ectOrientFeedbackBR 

signalSet ObjectPosControlBR 2 

objectPosControlXBR m/sec A 2 

objectPosControlYBR m/sec A 2 

signalSet ObjectOrientControlBR 1 

objectOrientControlBR l/sec A 2 

signalSet ObjectDestBRPos 2 

objectDestBRPosX meters 0 
objectDestBRPosY meters 0 

signalfeet ObjectWorkSpaceCenter 

objBaseX meters 0.48 

objBaseY meters 0.0 


2 
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E.4 Endpoint-Control Gains 

# Name : 

# endptControl.dat 

# 

# Description: 

# Endpoint control gains and signals. 

# 

# 


signalSet RAdesEndptPos 2 

RAdesEndptXPos meters 0 

RAdesEndptYPos meters 0.2 

signalSet RAdesEndptVel 2 

RAdesEndptXVel m/s ec 

RAdesEndptYVel m/s ec 

signalSet RAdesEndptAcc 2 

RAdesEndptXAcc m/sec^2 

RAdesEndptYAcc m/sec"2 

signalSet LAdesEndptPos 2 

LAdesEndptXPos meters 0 

LAdesEndptYPos meters -0.2 

signalSet LAdesEndptVel 2 

LAdesEndptXVel m/sec 

LAdesEndptYVel m/sec 

signalSet LAdesEndptAcc 2 

LAdesEndptXAcc m/sec"2 

LAdesEndptYAcc m/sec~2 

signalSet RAendptPosGains 2 

RAendptPosXGain 20 

RAendptPosYGain 20 

signalSet RAendptVelGains 2 

RAendptVelXGain 6 

RAendptVelYGain 6 

signalSet RAendptlntGains 2 

RAendptlntXGain 40 

RAendptlntYGain 40 

signalSet LAendptPosGains 2 

LAendptPosXGain 20 

LAendptPosYGain 20 

signalSet LAendptVelGains 2 

LAendptVelXGain 6 

LAendptVelYGain 6 

LAendptlntGains 2 


signalSet 
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LAendptlntXGain 40 

LAendptlntYGain 40 

signalSet RAendptFeedback 2 

RAendp txf eedback 
RAendptYfeedback 

signalSet LAendptFeedback 2 

LAendp t X f e e dba c k 
LAendptYf eedback 

signalSet RAendptControl 2 

RAendptXcontrol m/sec~2 

RAendptYcontrol m/sec"2 

signalSet LAendptControl 2 

LAendptXcontrol m/sec"2 

LAendptYcontrol m/sec"2 


E.5 Joint-Control Gains 


November 1990 
May 1991 


signalSet RAJointAngleGains 2 
RAShlderPosGain 8 

RAElbowPosGain 2 

signalSet LAJointAngleGains 2 
LAShlderPosGain 8 

LAE lbowPos Gain 2 

signalSet RAJointRateGains 2 

RAShlderVelGain 0 . 7 

RAElbowVelGain 0 . 5 

signalSet LAJointRateGains 2 

LAShlderVelGain 0 . 7 

LAElbowVelGain 0 . 5 

signalSet RAdesJointAngles 2 

RAdesRtShlderPos rad -1 

RAdesRtElbowPos rad 1 

signalSet LAdesJointAngles 2 


# Name : 

# jointPDcontrol.dat 

# 

# Description: 

# 

# Written by: Vincent Chen 

# Revised: Vincent Chen 

# Modified for BTController . 

# 

# 
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LAdesRtShlderPos rad 1 

LAdesRtElbowPos rad -1 

signalSet RAdesJointRates 2 

RAdesRtShlderVel rad/sec 0 

RAdesRtElbowVel rad/sec 0 

signalSet LAdesJointRates 2 

LAdesRtShlderVel rad/sec 0 

LAdesRtElbowVel rad/ sec 0 

signalSet RAdesJointAccel 2 

RAdesShlderAcc rad/sec~2 

RAdesElbowAcc rad/sec"2 

signalSet LAdesJointAccel 2 

LAdesShlderAcc rad/sec^2 

LAdesElbowAcc rad/sec~2 


PDdataSet 
pos : 
vel : 
output : 

Kp: 

Kv: 

desPos : 
desVel : 
orientation: 


RAJointPDcont roller 
RAJoint Angles 
RAJointRates 
RAJointTorques 
RAJo i n t Ang 1 eGa i ns 
RAJointRateGains 
RAdes Joint Angles 
RAdesJointRates 
0 


2 


PDdataSet 
pos : 
vel : 
output : 

Kp: 

Kv: 

desPos : 
desVel : 
orientation 


LAJointPDcont roller 
LAJoint Angles 
LAJointRates 
LAJointTorques 
LAJointAngleGains 
LAJointRateGains 
LAdes Joint Angles 
LAdesJointRates 
0 


2 


Sample 


Sample 


E.6 Base-Control Gains 


# Name : 

# baseControl.dat 

# 

# Revised: Vincent Chen May 1991 

# Modified for own use. 

# 

signalSet DesBasePos 

desBasePosX meters 

desBasePosY meters 


2 
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signalSet DesBaseOrient 1 

desBaseOrient radians 

signalSet DesBaseVel 2 

desBaseVelX m/sec 

desBaseVelY m/s ec 

signalSet DesBaseAngVel 1 

desBaseAngVel rad/ sec 

signalSet DesBaseAcc 2 

desBaseAccX m/sec~2 

desBaseAccY m/sec~2 

signalSet DesBaseAngAcc 1 

desBaseAngAcc rad/sec"2 

signalSet BasePosGains 2 

basePosGainX 31 

basePosGainY 31 

signalSet BaseVelGains 2 

baseVelGainX 90 

baseVelGainY 90 


signalSet BaseOrientGains 1 

baseOrientGain 0.6 

signalSet BaseAngVelGains 1 

baseAngVelGain 2 . 0 

signalSet BasePosFeedback 2 

basePosFeedbackX 
basePosFeedbackY 

signalSet BaseOrientFeedback 1 

baseOrient Feedback 

signalSet BasePosControl 2 

basePosControlX m/sec~2 

basePosControlY m/sec"2 

signalSet BaseOrientControl 

baseOrientControl l/sec~2 


1 
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